自作 Turtlebot3 自律走行に向けたプログラム。

2. Python プログラム。
Python プログラムは、Gazebo turtlebot3 Burger Depth Camera Navigation Python Program. で紹介した、
~/catkin_ws/src/ros_start/scripts/multi_goals.py を少し改造して試してみます。

~/catkin_ws/src/ros_start/scripts/multi_goals1.py
#!/usr/bin/env python # -*- coding: utf-8 -*- # Copyright 2017 HyphaROS Workshop. # Developer: HaoChih, LIN (hypha.ros@gmail.com) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # # multi_goals1.py # https://www.programcreek.com/python/?code=tianbot%2Ftianbot_racecar%2Ftianbot_racecar-master%2Fracecar_navigation%2Fscript%2Fmulti_goals.py # https://www.programcreek.com/python/example/70252/geometry_msgs.msg.PoseStamped # # https://edu.gaitech.hk/turtlebot/map-navigation.html # # 速度 平滑化 # https://www.codetd.com/ja/article/10672026 # # ROS Pythonでオイラー角とクォータニオンの相互変換 # https://yura2.hateblo.jp/entry/2016/05/18/ROS_Python%E3%81%A7%E3%82%AA%E3%82%A4%E3%83%A9%E3%83%BC%E8%A7%92%E3%81%A8%E3%82%AF%E3%82%A9%E3%83%BC%E3%82%BF%E3%83%8B%E3%82%AA%E3%83%B3%E3%81%AE%E7%9B%B8%E4%BA%92%E5%A4%89%E6%8F%9B # # プログラムからナビゲーションを実行する方法 # https://robot.isc.chubu.ac.jp/?p=1261 # 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 import tf from geometry_msgs.msg import Quaternion from geometry_msgs.msg import Vector3 import math def euler_to_quaternion(euler): """Convert Euler Angles to Quaternion euler: geometry_msgs/Vector3 quaternion: geometry_msgs/Quaternion """ q = tf.transformations.quaternion_from_euler(euler.x, euler.y, euler.z) return Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) class MultiGoals: def __init__(self, goalList, retry, map_frame): 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) # params & variables self.goalList = goalList self.retry = retry self.goalId = 0 self.goalMsg = PoseStamped() self.goalMsg.header.frame_id = map_frame self.goalMsg.pose.orientation.z = 0.0 self.goalMsg.pose.orientation.w = 1.0 # Publish the first goal time.sleep(1) self.mloop() def statusCB(self, data): if data.status.status == 3: # reached self.sts=2 else: self.sts=3 rospy.loginfo("statusCB error ! data.status.status=%d", data.status.status) def mloop(self): self.sts=0 self.prev_id=0 self.r_ct=0 self.pub_sub(self.goalId) self.goalId = self.goalId + 1 self.sts=1 while(1): if self.sts==2: self.sts=0 go=1 self.r_ct =0 if self.goalId <= (len(self.goalList)-1): id = self.goalId self.goalId = self.goalId + 1 else: if self.retry == 1: self.goalId = 0 id = self.goalId else: go=0 self.sts=9 if go==1: self.prev_id=id self.pub_sub(id) self.sts=1 elif self.sts==3: self.r_ct = self.r_ct + 1 if self.r_ct < 2: self.pub_sub(self.prev_id) self.sts=1 else: self.sts=9 time.sleep(1) if self.sts==9: break sys.exit() def pub_sub(self,id): self.goalMsg.header.stamp = rospy.Time.now() self.goalMsg.pose.position.x = self.goalList[id][0] self.goalMsg.pose.position.y = self.goalList[id][1] #euler_to_quarternion(Vector3(0.0, 0.0, math.pi / 2.0)) q=euler_to_quaternion(Vector3(0.0, 0.0, self.goalList[id][2])) self.goalMsg.pose.orientation.x = q.x self.goalMsg.pose.orientation.y = q.y self.goalMsg.pose.orientation.z = q.z self.goalMsg.pose.orientation.w = q.w self.pub.publish(self.goalMsg) rospy.loginfo("Initial goal published! Goal ID is: %d", id) if __name__ == "__main__": try: # ROS Init rospy.init_node('multi_goals', anonymous=True) map_frame = rospy.get_param('~map_frame', 'map' ) retry = int(rospy.get_param('~retry', '0')) goalList =[[0.0,0.0, 0.0], [0.0,0.0, math.pi / 2.0], [0.0,0.0, math.pi ], [0.0,0.0, math.pi * 1.5 ], [0.0,0.0, 0.0], [2.0,0.0, 0.0], [2.0,0.0, math.pi / 2.0], [2.0,0.0, math.pi ], [0.0,0.0, math.pi ], [0.0,0.0, 0.0]] print goalList #sys.exit() if len(goalList) > 0: # Constract MultiGoals Obj rospy.loginfo("Multi Goals Executing...") mg = MultiGoals(goalList, retry, map_frame) rospy.spin() else: rospy.errinfo("Lengths of goal lists are not the same") except KeyboardInterrupt: print("shutting down")

動作は、
開始時に、１回転して、
2M 先へ行って、戻ってくる。だけです。

3. 起動方法。
1) Remote PC
$ roscore

2) SBC(Jetson Nano 2G)
$ sudo chmod 666 /dev/ttyTHS1
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch
Topic の一覧が出てくることを確認します。エラーになったら、一度　Ctl+Cで中止して、再度起動します。

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch rtabmap_ros_my rtabmap-nishi-stereoB_navigation3.launch SBC:=true

3) リモートPC
$ export TURTLEBOT3_MODEL=waffle
$ roslaunch rtabmap_ros_my rtabmap-nishi-stereoB_navigation3.launch

$ rosrun ros_start multi_goals1.py

4. 使ってみて。
Turtlebot3 の直進速度、回転速度は、なるべく遅くします。
~/catkin_ws/src/turtlebot3/turtlebot3_navigation/param/dwa_local_planner_params_waffle.yaml
DWAPlannerROS: # Robot Configuration Parameters #max_vel_x: 0.26 #min_vel_x: -0.26 # changed by nishi #max_vel_x: 0.11 #max_vel_x: 0.13 max_vel_x: 0.1 min_vel_x: -0.06 max_vel_y: 0.0 min_vel_y: 0.0 # The velocity when robot is moving in a straight line #max_vel_trans: 0.26 #min_vel_trans: 0.13 # changed by nishi #max_vel_trans: 0.22 #min_vel_trans: 0.11 # changed by nishi 2021.10.4 #max_vel_trans: 0.1 #max_vel_trans: 0.08 max_vel_trans: 0.06 min_vel_trans: 0.01 #max_vel_theta: 1.82 #min_vel_theta: 0.9 # changed by nishi #max_vel_theta: 0.7 #min_vel_theta: 0.3 # changed by nishi 2021.19.4 max_vel_theta: 0.2 min_vel_theta: 0.01 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: 2.0 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回転した時点でオドメトリーが狂うのでどうしようも無い。
Rviz のグリッドを見ていると、目的地点(2M 先)には、確かに到着しますが、
そのグリッド自体がずれてしまうので、実際もずれてた所に到着します。

対策。
Odometry と IMU のフュージョンを試してみます。
ROS講座61 位置情報の統合

注1) rtabmap-nishi-stereoB_navigation3.launch の rtabmap のパラメータは、 rtabmap_rosTutorialsStereoOutdoorMapping のほうが、
良いです。