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

class GenLine を作成します。

mower_robo1.py に続けて記述します。

~/catkin_ws/src/ros_start/scripts/mower_robo1.py  update 2020.12.5
... class GenLine: def __init__(self,get_map): self.get_map = get_map self.first_ob_all=0 self.lines = {} line_no=0 print 'self.get_map.grid.shape=',self.get_map.grid.shape (x,y) = self.get_map.grid.shape print '(x,y)=',x,y # 行を処理します。 for row in range(y): dt = self.get_map.grid[row] if dt.min() <= get_map.free_thresh: st=0 # region start -> 1 first=0 last=0 print 'row=',row for colm in range(x): if st == 0: if dt[colm] <= get_map.free_thresh: st=1 first=colm last=colm else: if dt[colm] > get_map.free_thresh: fs=[line_no,first,row,last,row] # first,last -> x ,row -> y print 'fs=',fs self.lines[line_no]=[first,row,last,row] # first,last -> x ,row -> y st =0 line_no += 1 else: last=colm # ラインが閉じていません if st == 1: fs=[line_no,first,row,last,row] print 'fs=',fs self.lines[line_no]=[first,row,last,row] # first,last -> x ,row -> y st =0 line_no += 1 def conv_dot2grid(self,x,y): return self.get_map.conv_dot2grid(x,y) def get_near_line(self,gx,gy): lng = 1000.0 f_or_l = None ln = None for key in self.lines.keys(): (sx,sy,ex,ey) = self.lines[key] #print 'sx,sy,ex,ey=',sx,sy,ex,ey lng0 = (gx-sx)**2 + (gy-sy)**2 lng0 = math.sqrt(lng0) if lng0 < lng: #print 'lng0,lng=',lng0,lng f_or_l = 0 # set start grid ln = key lng = lng0 lng0= (gx-ex)**2 + (gy-ey)**2 lng0 = math.sqrt(lng0) if lng0 < lng: #print 'lng0,lng=',lng0,lng f_or_l = 1 # set end grid ln = key lng = lng0 return ln,f_or_l def conv_line2grid(self,line_no,f_or_l): (sx,sy,ex,ey) = self.lines[line_no] if f_or_l == 0: gx = sx gy = sy else: gx = ex gy = ey return gx,gy ''' グリッドをメートル空間に変換 gx,gy は、左上が、原点 (0,0) ''' def conv_grid2meter(self,gx,gy,f_or_l): return self.get_map.conv_grid2meter(gx,gy,f_or_l) def del_line(self,line_no): del self.lines[line_no] def get_arrow_dir(self,f_or_l): return self.get_map.get_arrow_dir(f_or_l)

2. 最後に、移動 (草刈り) 処理
プログラムのメインの制御と、main を作成します。
class MowerRobo と main です。

mower_robo1.py に続けて記述します。

~/catkin_ws/src/ros_start/scripts/mower_robo1.py update 2020.12.5
... class MowerRobo: def __init__(self, retry, map_frame, gen_line): # params & variables self.st=0 # 0: 別ラインへ移動 1:ライン上移動 2:終了 self.gen_line = gen_line # class GenLine self.from_gxy = None # 移動元の grid x,y self.to_gxy = None # 移動先の grid x,y self.f_or_l = None # 0:start grid 1:end_grid self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10) self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) (gx,gy) = self.gen_line.conv_dot2grid(-0.9,-0.8) (self.line_no,self.f_or_l) = self.gen_line.get_near_line(gx,gy) #最初のラインと位置を取得 print 'self.line_no,self.f_or_l=',self.line_no,self.f_or_l self.to_gxy = self.gen_line.conv_line2grid(self.line_no,self.f_or_l) print 'self.to_gxy=',self.to_gxy (x,y) = self.gen_line.conv_grid2meter(self.to_gxy[0],self.to_gxy[1],self.f_or_l) self.zw = self.gen_line.get_arrow_dir(self.f_or_l) # 矢印の向きを取得 print 'x,y=',x,y self.retry = retry self.goalId = 1 self.goalMsg = PoseStamped() # https://answers.ros.org/question/34684/header-frame_id/ self.goalMsg.header.frame_id = map_frame # Publish the first goal time.sleep(1) self.prev_xy = self.get_odom() # 現在位置を保存 self.goalMsg.header.stamp = rospy.Time.now() self.goalMsg.pose.position.x =y self.goalMsg.pose.position.y =x self.goalMsg.pose.orientation.z = self.zw[0] self.goalMsg.pose.orientation.w = self.zw[1] self.pub.publish(self.goalMsg) rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId) self.goalId = self.goalId + 1 self.end_f=False def statusCB(self, data): if data.status.status == 3 : # reached now_xy=self.get_odom() #別グリッドなのに、実際は、無移動 if self.prev_xy == now_xy and self.from_gxy != self.to_gxy: print 'not move!! yet' (x,y) = self.gen_line.conv_grid2meter(self.to_gxy[0],self.to_gxy[1],self.f_or_l) # 実際の移動OK else: if self.st==0: # 別ラインへ移動中 if self.f_or_l == 0: self.f_or_l=1 else: self.f_or_l=0 self.from_gxy = self.to_gxy # 旧目的地グリッドを出発地グリッドとする self.to_gxy = self.gen_line.conv_line2grid(self.line_no,self.f_or_l) # ラインのもう一方のグリッドを新たな目的地グリッドとする print 'self.to_gxy=',self.to_gxy (x,y) = self.gen_line.conv_grid2meter(self.to_gxy[0],self.to_gxy[1],self.f_or_l) self.st=1 elif self.st==1: # ライン上を移動中 self.gen_line.del_line(self.line_no) # 次の最短ラインを取得 (self.line_no,self.f_or_l) = self.gen_line.get_near_line(self.to_gxy[0],self.to_gxy[1]) if self.line_no == None: self.st=2 print 'end!!' else: self.zw = self.gen_line.get_arrow_dir(self.f_or_l) # 矢印の向きを取得 self.from_gxy = self.to_gxy # 旧目的地グリッドを出発地グリッドとする self.to_gxy = self.gen_line.conv_line2grid(self.line_no,self.f_or_l) # 新たな目的地グリッドを取得 print 'self.to_gxy=',self.to_gxy (x,y) = self.gen_line.conv_grid2meter(self.to_gxy[0],self.to_gxy[1],self.f_or_l) self.st=0 self.prev_xy = now_xy if self.st != 2: print '(x,y)=',x,y print 'self.from_gxy=',self.from_gxy,',self.to_gxy=',self.to_gxy self.goalMsg.header.stamp = rospy.Time.now() self.goalMsg.pose.position.x =y self.goalMsg.pose.position.y =x self.goalMsg.pose.orientation.z = self.zw[0] self.goalMsg.pose.orientation.w = self.zw[1] self.pub.publish(self.goalMsg) rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId) self.goalId = self.goalId + 1 else: rospy.loginfo("Get Error is: %d", data.status.status) def get_odom(self): odom_msg=None cnt=30 x=None y=None while odom_msg is None and cnt >=0: try: odom_msg = rospy.wait_for_message('/odom', Odometry, timeout=5) except: pass cnt-=1 if odom_msg != None: x =int(odom_msg.pose.pose.position.x * 100.0) x = float(x) / 100.0 y =int(odom_msg.pose.pose.position.y * 100.0) y = float(y) / 100.0 print 'odom_msg x,y=',x,y print 'ok' else: print 'error' return x,y if __name__ == '__main__': try: # ROS Init rospy.init_node('mower_robo1', anonymous=True) get_map=GetMap() gen_line=GenLine(get_map) map_frame = rospy.get_param('~map_frame', 'map' ) retry = rospy.get_param('~retry', '1') rospy.loginfo("mower_robo1 Executing...") mr = MowerRobo(retry, map_frame,gen_line) rospy.spin() except KeyboardInterrupt: print("shutting down")

3. 実行です。

$ roscore

$ export TURTLEBOT3_MODEL=burger
$ roslaunch turtlebot3_gazebo turtlebot3_world_nav_nishi.launch

$ export TURTLEBOT3_MODEL=burger
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

$ roscd ros_start/scripts
$ python mower_robo1.py


以上です。
end !! が出たら、終点です。

まだまだ、荒削りですが、目標の動きは、再現できると思います。
途中で、壁にぶつかったら、 Rviz の 2D Navi Goal で少し移動させてやってみとうせ。!!

注1) たまに、目的地が飛ばされる時がある。
 self.pub.publish(self.goalMsg) の後で、move_base/result の空イベント上がってくるみたい!!
他の人も経験しているみたい。
https://github.com/jsk-ros-pkg/jsk_common/issues/155

これに関しては、/move_base/result のイベントが来たときに、/odom の pose.pose.position.x、y が変わってなかったら、
もう一度同じ目的地への移動メッセージを出すようにしました。
 当初、コールバックルーチンの中で、目的地への移動メッセージしていたのが良くないのかと、考えて、目的地への移動メッセージ をコールバックルーチン外で出す様に変更してテストしましたが、改善されませんでした。

また、/move_base_simple/goal へのメッセージの、WaitForTransform をしなければいけない書き込みも在りましたが、
これば、的外れのきがします。
/move_base_simple/goal へのメッセージ が届いているから、/move_base/result のコールバックが実行されているので。

update 2020.12.4 by nishi


注2) ラインのstart,end が壁に近すぎる事がある。
 今回、これの対策は組み込んでいないので、我慢しとうせ。

注3) 逆に、遠いときがある。
 まだ、プログラムを突き詰めていないので、勘弁しとうせ!!

注4) Depth カメラでの 障害物判定距離が近すぎる(まだ、遠くに設定できない)ので、
うまく、壁を避けられない時がある。
レーザーの方を使えば、問題ないと思うが、そうすると本来の趣旨から言ってどうか..

追々、改修していかんといかんぞね!!

いまは、Rviz で動きをじっくり観察して、どうするか考えているところじゃ!!
後、たまに誤字があるのは、勘弁しとうせ、 FiLCO のクソボロのキーボードが、
あまりにチャタリングがひどくてダブって入る所為じゃ。

注5) バーガーの車体サイズの定義が、ナビゲーションでは、大きのでは!
turtlebot3_navigation/param/costmap_common_params_burger.yaml
#footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
#changed by nishi
footprint: [[-0.110, -0.090], [-0.110, 0.090], [0.041, 0.090], [0.041, -0.090]]

注6) 壁に近い直線は、壁から離れて、カーブを描いたコースを取る。
 もしかしたら、到着時のバーガーの向きが適切でないかもしれない。
ここのプログラムは、まだ未対応だが、
草刈りなので、とにかく、真直ぐ進んで欲しい。草を障害物と判定したら、草刈りが出来ない。
やはり、基本、障害物を避けるナビゲーションではなくて、機械学習で、草を選んでコースを決めるロボットが必要か?

注7) /move_base/result の戻りコード
 status.status = 3 : 正常
 status.status = 2 : 目標地が再設定された
 status.status = 4 : バーガー糞詰り。ニッチもサッチも動かない。
  この場合は、手助けが必要。ここは、プログラムで救済方法を考えないといけない!!

注8) バーガーの向きを、ラインに一致するようにしました。update 2020.12.5 by nishi

注9) Map の役割があまり理解できなかったので、turtlebot3_empty_world.launch を使って、ある意味レーザー、
Depth カメラが無いのと同じ状況にして試してみました。
他の、ナビゲーションの起動等は同じにして。
どうやら、次の目的地が与えられたときに、Map を元にした経路計画が作られる。..この点は有効に働いている。
この時、黒ラインが表示される。
黒ラインに従って移動するに従って、目的地の途中までの赤ラインが表示されて、それに従って移動する。

 ただし、移動途中で最短コース(この場合は、Map の壁は無視)が現れると、そのコースを選択して進むようになります。
今回は、センサーが無いのでそのまま突切って、目的地へ到着します。

この動きから、Map情報が何処まで使われているのか、推測できます。

この時、センサーが在れば、センサーに現れる障害物をうまく処理できれば、問題なく切り抜けられます。
ただし、この場合にうまくセンサー情報の対応処理がうまく出来ない時が有って、この場合最後までうまく行かないと、
rc=4 で移動を停止します。

要約すると、当初の経路計画には、確かに、Map が使われる。
ただし、移動するに連れて、初期の経路よりも、センサーを使った経路を重視するようになる。

問題箇所。
"ROS ロボットプログラミングバイブル" の本を参考に、この問題行動を推測すると、
11.6.1 ナビゲーション(Navigation) に記述のある、
動作計画(Motion Planning)、移動/障害物回避(Move/Collision Avoidance) に記述のある、
move_baseの経路計画パッケージの中の、障害物回避アルゴリズム Dynamic Window Approach (DWA) に問題があると思われます。
パラメータ不良か、プログラム自体の不具合か?


注10) /move_base_simple/goal のリクエストに対して、動作完了(status=3)、エラー(status=4) になると、
/cmd_vel がプログラムで使えるようになるので、この間に、プログラムでバーガーを自由に操作できるみたいぞね!!
と言うことは、目的地までナビゲーションを利用して、目的地に到着したら、自分でバーガーを操作して何かできると言うことか。

総括.
草刈りロボットは、vel_bumper.py の様な、とりあえず真直ぐ進んで、障害物がバンパーに当たったら、
回避行動をする様な物が適しているのか?
そのバンパー役を、カメラで写して機械学習で判定する類が向いているのかも。

Gazebo のシュミレーションで、この事が簡単に学習出来たのでよかったです。

その後。
畑の草刈りロボットであれば、畝や、畑が突然崖になっていたりするので、そこを認識するには、
やはり 3D Map でないといけない。
RTB-Map 3D で、屋外でActive SLAM を行っている例が、You Tube に出ているので、試してみたいぞね。
rtabmap_ros
その前に、 RTB-Map で、2D Map の作成は、簡単に試せました。
Mapping and Navigation with Turtlebot
でも、PC のパワーが結構、必要みたいです。