Gazebo turtlebot3 Burger Depth Camera Navigation 草刈りロボット #2

/map nav_msgs/OccupancyGrid とのことなので、インターネットで検索して、なんとか作って見ました。

~/catkin_ws/src/ros_start/scripts/map_test1.py
#!/usr/bin/env python # -*- coding: utf-8 -*- ''' map_test1.py https://www.programcreek.com/python/example/95997/nav_msgs.msg.OccupancyGrid https://gist.github.com/chrissunny94/16752edbea0492ee32b2705c5d72cbad numpy to opencv https://note.nkmk.me/python-opencv-numpy-color-to-gray/ https://stackoverflow.com/questions/24124458/how-to-change-numpy-array-into-grayscale-opencv-image ''' import rospy import numpy as np import cv2 from nav_msgs.msg import OccupancyGrid def callback(data): #rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data) print 'data.info=',data.info print 'data.data[0:10]=',data.data[1:10] print 'ok' def listener(): rospy.init_node('listener', anonymous=True) rospy.Subscriber("/map", OccupancyGrid, callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin() def conv_fmt2(map_msg): ogrid = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width)) #print ogrid.shape #print ogrid np.place(ogrid,ogrid == -1, 255) # replace all -1 to 255 ogrid = ogrid.astype(np.uint8) if True: cv2.imwrite('xxx2.jpg', ogrid) cv2.namedWindow("image",cv2.WINDOW_NORMAL) cv2.imshow("image", ogrid) cv2.waitKey(0) cv2.destroyAllWindows() height = int(map_msg.info.height/4) width = int(map_msg.info.width/4) img2 = cv2.resize(ogrid,(width,height)) # resize map_msg.data print 'img2.shape=',img2.shape if False: cv2.imwrite('xxx4.jpg', img2) cv2.namedWindow("image",cv2.WINDOW_NORMAL) cv2.imshow("image", img2) cv2.waitKey(0) cv2.destroyAllWindows() def get_map(): map_msg=None cnt=30 while map_msg is None and cnt >=0: try: map_msg = rospy.wait_for_message('/map', OccupancyGrid, timeout=5) except: pass cnt-=1 if map_msg != None: print 'map_msg.info=',map_msg.info #print 'map_msg.data[0:10]=',map_msg.data[1:10] conv_fmt2(map_msg) print 'ok' else: print 'error' if __name__ == '__main__': rospy.init_node('map_test1') #listener() get_map()

実行は、下記になります。

$ roscd ros_start/scripts $ python map_test1.py map_msg.info= map_load_time: secs: 12 nsecs: 853000000 resolution: 0.0500000007451 width: 421 height: 421 origin: position: x: -10.525 y: -10.525 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 img2.shape= (105, 105) ok

実行すると、/map で出てくるマップ(pgm) が、OpenCV で確認できます。
Rviz で見る地図と比べてみると、同じでありません。
どうやら、 y=x の線での線対称になっているようです。
これが、こんかいの曲者でした。

width: 421
height: 421
resolution: 0.05
なので、21.05 X 21.05 [meter] の地図と言うことです。
地図の原点は、真ん中 : (x,y)=(210,210) [dot] = M(0,0) [meter] になるようです。
Gazebo で見ると、青、赤、緑のラインが出ているので、そこが中心でした。
もっと、早く気が付けばよかったぞね!!

もちろん、Map画像では、左上が、(x,y) = (0,0) です。

Rivz の地図では、
(1) 左上 M(+x,+y)  (4) 右上 M(-x,+y)
(2) 左下 M(+x,-y)  (3) 右下 M(-x,-y)

注) M() は、Meter 空間と言う意味にします。
  +x,+y = 10.525 [meter] (+210 [dot])
  -x,-y = -10.525 [meter] (-210 [dot])

これが、Map データ上では、
(3) 左上 M(-x,-y)  (4) 右上 M(-x,+y)
(2) 左下 M(+x,-y)  (1) 右下 M(+x,+y)

注1) (n) は、図面上の部屋の４角に当たる所に、番号を振りました。
Rviz の図面の左上から、反時計回りに、４角に割り振っています。
Rvizの角に対応する、Mapデータ角が、同じ(n) になります。

Rviz と、Mapデータでは、 (1) と (3) の角が入れ替わっています。
つまり、 直線 y=x での、線対称のようです。
Mapデータ の x,y を入れ替えれば、Rviz のデータになります。
つまり、 Mapデータ(x,y) の位置は、 M(y,x) の位置にあたります。

上のM(+x,+y) M(-x,-y)等を書いたのは、早見のためです。
Mapデータの原点は、左上なので、
3) 左上 M(-x,-y) にあたります。この場合、
x=0 は、 -210 から、y=0 は、-210 からそれぞれ軸に沿って変化すると言う意味です。
超、頭が痛くなるぞね!!

今後、Map データを、 Numpy 配列に入れて、操作しますが、
その際には、あくまでも、通常の(x,y) 配列として、内部処理をして、
最後に、move_base_simple/goal へPublish するときに、x,y を入れ替えします。

注2) Mapデータを、OpenCV で、上下反転、回転して、Rviz と同じ画像にしても、OKぞね。
 最初に試したのは、この方法でしたが、うまく行かないと思ったのですが、結局は、OKでした。
こちらの方が視覚上同じなので、便利かも。
でも、意図的に、線対称のデータにしてあるのであれば、何か意味があるような気がしてなりましぇん。

2. 実際のプログラムです。
最初は、 グリッド情報テーブル 作です。
class GetMap を作成します。

ちなみに、プログラム名は、mower_robo1.py です。

~/catkin_ws/src/ros_start/scripts/mower_robo1.py  update 2020.12.5
#!/usr/bin/env python # -*- coding: utf-8 -*- ''' mower_robo1c.py ''' import rospy import numpy as np import cv2 import string import math import time import sys from std_msgs.msg import String from move_base_msgs.msg import MoveBaseActionResult from actionlib_msgs.msg import GoalStatusArray from geometry_msgs.msg import PoseStamped from nav_msgs.msg import OccupancyGrid,Odometry class GetMap: def __init__(self): self.map_info = None self.map_data = None self.grid = None self.resolution = None #self.line_w = 4 # ラインの幅 -> grid size [dot] self.line_w = 10 # ラインの幅 -> grid size [dot] self.car_r = 2 # ロボットの回転半径 [dot] self.match_rviz = True # True / Flase -> Rviz の画像と同じにする / しない map_msg=None cnt=30 while map_msg is None and cnt >=0: try: map_msg = rospy.wait_for_message('/map', OccupancyGrid, timeout=5) except: pass cnt-=1 if map_msg != None: self.map_info = map_msg.info print 'map_info=',self.map_info self.free_thresh = int(0.196 * 255) self.resolution = self.map_info.resolution print 'self.resolution=',self.resolution self.org_x = self.map_info.origin.position.x self.org_y = self.map_info.origin.position.y print 'self.org_x=',self.org_x print 'self.org_y=',self.org_y x_size = 421 * self.resolution y_size = 421 * self.resolution print 'x_size,y_size=',x_size,y_size #print 'map_msg.data[0:10]=',map_msg.data[1:10] print 'self.free_thresh=',self.free_thresh self.conv_fmt2(map_msg) print 'ok' else: print 'error' def conv_fmt2(self,map_msg): m_grid = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width)) print m_grid.shape #print m_grid np.place(m_grid,m_grid == -1, 255) # replace all -1 to 255 m_grid = m_grid.astype(np.uint8) # ここで、Rviz の画像に合わせてみる。 if self.match_rviz == True: #m_grid = np.rot90(m_grid) # 90度回転 #m_grid = np.fliplr(m_grid) a_trans = m_grid.transpose() m_grid = a_trans m_grid = np.rot90(m_grid) # 90度回転 m_grid = np.rot90(m_grid) # 90度回転 if False: cv2.imwrite('xxx3.jpg', m_grid) cv2.namedWindow("image",cv2.WINDOW_NORMAL) cv2.imshow("image", m_grid) cv2.waitKey(0) cv2.destroyAllWindows() self.map_data = m_grid height = int(map_msg.info.height / self.line_w) width = int(map_msg.info.width / self.line_w) print 'map_msg.info.height,map_msg.info.width=',map_msg.info.height,map_msg.info.width print 'height,width=',height,width #self.grid = cv2.resize(m_grid,(width,height)) # resize map_msg.data self.grid = self.np_resize(m_grid,(width,height)) #np.place(self.grid,self.grid <= self.free_thresh ,0) #np.place(self.grid,self.grid > self.free_thresh ,255) print 'self.grid.shape=',self.grid.shape if False: cv2.imwrite('xxx4.jpg', self.grid) cv2.namedWindow("image",cv2.WINDOW_NORMAL) cv2.imshow("image", self.grid) cv2.waitKey(0) cv2.destroyAllWindows() def conv_dot2grid(self,x,y): xi= int(x / self.resolution) yi= int(y/ self.resolution) gx = int(xi / self.line_w) gy = int(yi / self.line_w) return gx,gy ''' convert Grid to Meter World ''' def conv_grid2meter(self,gx,gy,f_or_l): if self.match_rviz == True: return self.conv_grid2meter_m_p(gx,gy,f_or_l) else: return self.conv_grid2meter_m_m(gx,gy,f_or_l) ''' Rviz と同じ図形 にした場合。 左上: M(+x,+y) 右上: M(-x,+y) 左下: M(+x,-y) 右下: M(-x,-y) ''' def conv_grid2meter_m_p(self,gx,gy,f_or_l): #x = (210 - (gx * self.line_w + self.line_w/2)) * self.resolution x = -(gx * self.line_w + self.line_w/2) * self.resolution - self.org_x # chnaged by nishi 2020.12.2 #y = (210 - (gy * self.line_w + self.line_w/2)) * self.resolution y = -(gy * self.line_w + self.line_w/2) * self.resolution - self.org_y # chnaged by nishi 2020.12.2 return x,y ''' Map データの図形 : y = x の線対称の図形(x,y の入れ替え) 左上: M(-x,-y) 右上: M(-x,+y) 左下: M(+x,-y) 右下: M(+x,+y) ''' def conv_grid2meter_m_m(self,gx,gy,f_or_l): #x = (-210 + (gx * self.line_w + self.line_w/2)) * self.resolution x = (gx * self.line_w + self.line_w/2) * self.resolution + self.org_x # chnaged by nishi 2020.12.2 #y = (-210 + (gy * self.line_w + self.line_w/2)) * self.resolution y = (gy * self.line_w + self.line_w/2) * self.resolution + self.org_y # chnaged by nishi 2020.12.2 return y,x ''' get Arrow direction f_or_l # 0:start grid 1:end_grid ラインが移った時のみ設定する事。 ''' def get_arrow_dir(self,f_or_l): if self.match_rviz == True: if f_or_l == 0: # start grid --> right arrow z= -0.7 w=0.7 else: # end grid --> left arrow z= 0.7 w=0.7 else: if f_or_l == 0: # start grid --> down arrow z = 1.0 w= 0.0 else: # end grid --> up arrow z = 0.0 w = 1.0 return z,w def get_rotation_matrix(self,rad): """ 指定したradの回転行列を返す """ rot = np.array([[np.cos(rad), -np.sin(rad)], [np.sin(rad), np.cos(rad)]]) return rot def np_resize(self,m_grid,req_size): (w,h) = req_size (w0,h0) = m_grid.shape size_x = int(w0 / w) size_y = int(h0 / h) o_dt = np.zeros((w, h),dtype='uint8') for i in range(w): for j in range(h): #av = np.mean(m_grid[i*size_x:(i+1)*size_x,j*size_y:(j+1)*size_y]) av = np.max(m_grid[i*size_x:(i+1)*size_x,j*size_y:(j+1)*size_y]) # changed by nishi 2020.12.2 #if av < 255: # print 'av=',av if math.isnan(av) == False: o_dt[i,j]= int(av) if o_dt[i,j] < 255: print 'o_dt[i,j]=',o_dt[i,j] else: print 'GetMap.np_resize() : resize error' return o_dt