Gazebo turtlebot3 Burger Depth Camera Navigation Python Program.

1. 作成。
そのままでは、家から、はみ出すので、ちょっとだけ変更します。
~/catkin_ws/src/ros_start/scripts/multi_goals.py
... if __name__ == "__main__": try: # ROS Init rospy.init_node('multi_goals', anonymous=True) # Get params #goalListX = rospy.get_param('~goalListX', '[2.0, 2.0]') goalListX = rospy.get_param('~goalListX', '[-1.36, 0.0]') #goalListY = rospy.get_param('~goalListY', '[2.0, 4.0]') goalListY = rospy.get_param('~goalListY', '[2.75, 0.0]') map_frame = rospy.get_param('~map_frame', 'map' ) retry = rospy.get_param('~retry', '1') ....
うむ、動いたぞね!!
障害物があって、バーガーが途中で停止したりする場合があるので、それへの対応処理を組み込めば、
使えるようになりそうぞね。

2. 経路の計画。
 次は、経路の計画を立ててみます。
それらしいツールが、まだ見つけられないので、ここは、ちょっと頭をひねって。
Rviz で 2D Nav Goal を行うと、/move_base_simple/goal が出てくるので、これを、Python プログラムで逐次チェックして、
ファイルにお落とすようにします。

そして、そのファイルを、上記プログラムを少し手直しして、ファイルから目的地データを取り込んで、バーガー を動かします。
そうすれば、Rviz で 経路の計画が立てられるのでは無いか、と言う考えです。

同じ発想の人も、いるもんです。
Map-Based Navigation
 Finding the Coordinates for Locations of Interest
注) おんちゃんは、この人の考えを流用したのではないぞね、自分で考えたぞね!!
その点は、間違えないでいとうせ!!

3. Rviz で目的地を指定してルートを作成。
~/catkin_ws/src/ros_start/scripts/tbot3_sav_move_base_simple.py
#!/usr/bin/env python # -*- coding: utf-8 -*- # tbot3_sav_move_base_simple.py # import rospy import string import math import time import sys from std_msgs.msg import String from geometry_msgs.msg import PoseStamped class Sav_Goal: def __init__(self): self.s_file = 'route_plan.txt' self.flg=False self.sub = rospy.Subscriber('move_base_simple/goal', PoseStamped, self.statusCB ,queue_size=10) def statusCB(self, data): #print 'data=',data print 'data.pose.position.x=',data.pose.position.x print 'data.pose.position.y=',data.pose.position.y print 'data.pose.orientation.z=',data.pose.orientation.z print 'data.pose.orientation.w=',data.pose.orientation.w if self.flg==False: f = open(self.s_file,mode='w') self.flg=True else: f = open(self.s_file,mode='a') f.write(str(data.pose.position.x)) f.write(',') f.write(str(data.pose.position.y)) f.write(',') f.write(str(data.pose.orientation.z)) f.write(',') f.write(str(data.pose.orientation.w)) f.write("\n") f.close() if __name__ == "__main__": try: # ROS Init rospy.init_node('tbot3_sav_move_base_simple', anonymous=True) rospy.loginfo("tbot3_sav_move_base_simple.py Executing...") sav_goal =Sav_Goal() rospy.spin() except KeyboardInterrupt: print("shutting down") <textarea> </p>

3.1 操作
$ 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/map6.yaml

$ roscd ros_start/scripts
$ python tbot3_sav_move_base_simple.py

Rviz で目的地を順に指定して、ルートを作成する。
ルートが、完成したら、tbot3_sav_move_base_simple.py を終了させます。
route_plan.txt が作成されています。

4. 保存されたルートを読みこんで、それをたどるプログラム。
~/catkin_ws/src/ros_start/scripts/tbot3_nav1.py
#!/usr/bin/env python # -*- coding: utf-8 -*- ''' tbot3_nav1.py https://www.programcreek.com/python/example/70252/geometry_msgs.msg.PoseStamped https://www.programcreek.com/python/?code=tianbot%2Ftianbot_racecar%2Ftianbot_racecar-master%2Fracecar_navigation%2Fscript%2Fmulti_goals.py ''' import rospy 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 class MultiGoalsEx: def __init__(self, retry, map_frame): # params & variables self.s_file = 'route_plan.txt' self.route_plan=[] with open(self.s_file) as f: line = f.readlines() for s in line: s= s.rstrip('\n') print 's=',s (x,y,z,w) = s.split(",") self.route_plan.append([float(x),float(y),float(z),float(w)]) 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) self.retry = retry self.goalId = 0 self.goalMsg = PoseStamped() self.goalMsg.header.frame_id = map_frame # Publish the first goal time.sleep(1) self.goalMsg.header.stamp = rospy.Time.now() self.goalMsg.pose.position.x = self.route_plan[self.goalId][0] self.goalMsg.pose.position.y = self.route_plan[self.goalId][1] self.goalMsg.pose.orientation.z = self.route_plan[self.goalId][2] self.goalMsg.pose.orientation.w = self.route_plan[self.goalId][3] 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 if self.end_f != True: self.goalMsg.header.stamp = rospy.Time.now() self.goalMsg.pose.position.x = self.route_plan[self.goalId][0] self.goalMsg.pose.position.y = self.route_plan[self.goalId][1] self.goalMsg.pose.orientation.z = self.route_plan[self.goalId][2] self.goalMsg.pose.orientation.w = self.route_plan[self.goalId][3] self.pub.publish(self.goalMsg) rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId) if self.goalId < (len(self.route_plan)-1): self.goalId = self.goalId + 1 else: self.goalId = 0 if self.retry != 1: self.end_f = True else: rospy.loginfo("Get Error is: %d", data.status.status) if __name__ == "__main__": try: # ROS Init rospy.init_node('multi_goals', anonymous=True) map_frame = rospy.get_param('~map_frame', 'map' ) retry = rospy.get_param('~retry', '1') rospy.loginfo("Multi Goals Executing...") mg = MultiGoalsEx(retry, map_frame) rospy.spin() except KeyboardInterrupt: print("shutting down")

4.1 操作
$ 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/map6.yaml

$ roscd ros_start/scripts
$ python tbot3_nav1.py

route_plan.txt を読み込んで、ルートを辿ります。

5. 感想
1) どうしても後ろ向きに進むシーンが多い。
 離れた地点を指定すると、どうしてもそのまま後ろ向きに移動して、到着点で、指定の向きに変わる。
後ろ向きに進まれると、Depth カメラで障害物を見つけられない。

あくまでも、レーザーセンサーを前提の動きぞね!!
まだ、設定パラメータに、Depth カメラ用の項目があるのか?

2) 取るコースが直線的でない。
 多分、進路変更のアルゴリズムが、直角コーナを例に取れば、その場でクルッと回る
のではなくて、車のように前に進みながらコーナーを曲がるので、ベジェ曲線の様な進路変更を取る、作りぞね。

これでは、おんちゃんの目的の草刈りロボットの進み方には、ちょっと合わない。
その場で、クルッと回って、後は、まっすぐ目的地に行って欲しい。

注1) マップ情報で、壁に近い直進のルートの場合、その壁を避ける形で、遠回りのルートを、選んでいる様にも見える。

6. 設定情報を見直しました。

1) turtlebot3/turtlebot3_navigation/param/costmap_common_params_burger.yaml
#obstacle_range: 3.0 # changed by nishi obstacle_range: 0.31 raytrace_range: 3.5 footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] #robot_radius: 0.105 #inflation_radius: 1.0 # changed by nishi inflation_radius: 0.17 cost_scaling_factor: 3.0 map_type: costmap #observation_sources: scan #scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} # test by nishi observation_sources: point_cloud_snsor point_cloud_snsor: {sensor_frame: camera_link, data_type: PointCloud2, topic: camera/depth/points, marking: true, clearing: true}
注1)
obstacle_range: 1.0 [m] 位にしたいのだが、そうすると、Depth Camera なので、地面を障害物として、捉えるみたいで、うまく行かない。
Depth Camera の PointCloud2 の Y軸を制限するには、どうすればよいのか?

注2)
#inflation_radius: 1.0 では、おんちゃんの場合は、距離が有り過ぎでした。

2) turtlebot3/turtlebot3_navigation/param/dwa_local_planner_params_burger.yaml
DWAPlannerROS: # Robot Configuration Parameters #max_vel_x: 0.22 #min_vel_x: -0.22 # changed by nishi max_vel_x: 0.13 min_vel_x: -0.03 max_vel_y: 0.0 min_vel_y: 0.0 # The velocity when robot is moving in a straight line max_vel_trans: 0.22 min_vel_trans: 0.11 #max_vel_theta: 2.75 #min_vel_theta: 1.37 # changed by nishi max_vel_theta: 1.0 min_vel_theta: 0.3 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 # Goal Tolerance Parametes xy_goal_tolerance: 0.05 yaw_goal_tolerance: 0.17 latch_xy_goal_tolerance: false # Forward Simulation Parameters sim_time: 1.5 vx_samples: 20 vy_samples: 0 vth_samples: 40 controller_frequency: 10.0 # Trajectory Scoring Parameters path_distance_bias: 32.0 goal_distance_bias: 20.0 occdist_scale: 0.02 forward_point_distance: 0.325 stop_time_buffer: 0.2 scaling_speed: 0.25 max_scaling_factor: 0.2 # Oscillation Prevention Parameters oscillation_reset_dist: 0.05 # Debugging publish_traj_pc : true publish_cost_grid_pc: true
注1) Burger が後ろ向きに進む件。
#min_vel_x:-0.22
# ここがマイナスだと、バックするみたい。0.0 だと、バックしないが、障害物にぶつかった時、回避行動のバックができない。
min_vel_x:-0.03 # これ位か?

初期値が、
max_vel_x: 0.22
min_vel_x: -0.22
だったので、前進、後進のどちらを選択しても良い。と判断していたのでは。

注2) Bureger が、大回りになる件。
#max_vel_theta: 2.75
#min_vel_theta: 1.37
# ここが大きいと大回りになるみたい。
max_vel_theta: 1.0
min_vel_theta: 0.3

この値を調整すれば、その場でクルッと向きを変えるのでは?

この2つのファイルの設定変更で、だいぶ、オンちゃんが考える、草刈りロボットの動きに近づいてきたぞね!!

Gazebo 上のシュミレーションでも、得られる情報は、結構あるもんじゃ。
実機でテストする前に、ここまで判れば十分じゃ。

後は、草刈りで、畑をくまなく移動させるにはどうすればよいか?