move_base parameters when odometry is perfect?
Hello ROS Fans,
For purposes of testing, I am trying to use move_base together with the ArbotiX differential drive controller in "fake simulation" mode to move a simulated TurtleBot along a 1 meter square. In simulation mode, the ArbotiX differential controller moves the robot exactly where you tell it so that odometry is essentially perfect. So there is no physics involved as when using Gazebo.
I want to move a simulated TurtleBot under these conditions along a 1 meter square as accurately as possible. Using a script that directly monitors the /odom topic and uses Twist commands to move the robot, I can get a pretty good result as shown in this image:
However, if I use move_base and a blank map to try to accomplish the same thing, I'm having a lot of trouble finding parameters that give a good result. Here for example is the best I've been able to do so far:
I am using tolerances of 0.05 meters for xy and 0.087 radians for yaw. I would have thought that with perfect odometry, it would be relatively easy for move_base to hit these tolerances but as the image above shows, there is a lot of slop in the trajectory. No doubt I am missing something basic but I can't figure it out.
Below are the config files I am using. After those is the script I am using to move the robot in a square:
base_local_planner_params.yaml:
controller_frequency: 20.0
#planner_patience: 5.0
#controller_patience: 5.0
TrajectoryPlannerROS:
max_vel_x: 0.2
min_vel_x: 0.05
max_rotational_vel: 1.0
min_in_place_rotational_vel: 0.2
escape_vel: -0.10
acc_lim_th: 0.8
acc_lim_x: 0.2
acc_lim_y: 0.2
holonomic_robot: false
yaw_goal_tolerance: 0.087 # about 5 degrees
xy_goal_tolerance: 0.05 # 5 cm
goal_distance_bias: 0.6
path_distance_bias: 0.8
heading_lookahead: 0.325
meter_scoring: true
heading_scoring: false
oscillation_reset_dist: 0.05
occdist_scale: 0.05
publish_cost_grid_pc: false
sim_time: 1.0
sim_granularity: 0.01
angular_sim_granularity: 0.01
vx_samples: 6
vtheta_samples: 20
dwa: false
costmap_common_params.yaml:
obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.15
inflation_radius: 0.5
max_obstacle_height: 0.6
min_obstacle_height: 0.08
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true}
global_costmap_params.yaml:
global_costmap:
global_frame: /odom
robot_base_frame: /base_link
update_frequency: 20.0
publish_frequency: 1.0
static_map: false
rolling_window: true
width: 10.0
height: 10.0
resolution: 0.05
transform_tolerance: 1.0
local_costmap_params.yaml:
local_costmap:
global_frame: /odom
robot_base_frame: /base_link
update_frequency: 20.0
publish_frequency: 1.0
static_map: false
rolling_window: true
width: 1.0
height: 1.0
resolution: 0.05
transform_tolerance: 1.0
blank_map.yaml:
image: blank_map.pgm
resolution: 0.05
origin: [-10, -10, 0]
occupied_thresh: 1.0
free_thresh: 1.0
negate: 0
move_base_square.py:
#!/usr/bin/python
import roslib
roslib.load_manifest('rbx_nav')
import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from math import radians
class MoveBaseSquare():
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
square_size = 1.0 # meters
turn_angle = radians ...
Could you try a path that doesn't require a "spin-in-place" operation and see if you get the results you expect? base_local_planner's trajectory scoring function doesn't work well for choosing pivot turns.
Patrick, could you officially post the answer to the question?