About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Configure and run Robot Navigation

Description: In this tutorial you will learn how to set up the teb_local_planner as local planner plugin for the navigation stack.

Keywords: Trajectory Optimization Local Planner Navigation

Tutorial Level: INTERMEDIATE

Next Tutorial: Obstacle Avoidance and Robot Footprint Model

Set up your Robot for 2D Navigation

Please make sure to setup your robot to comply with the 2d navigation package (sensors, costmap2d and robot driver/simulator). Refer to the following tutorial: Navigation: RobotSetup.

Afterwards, you might have created a tiny package containing configurations and launch files for your robot navigation tasks. The main configuration files are: costmap_common_params.yaml, global_costmap_params.yaml, local_costmap_params.yaml and base_local_planner_params.yaml.

Additionally, the package should contain a launch file move_base.launch similar to the following one:

<launch>
  <master auto="start"/>

  <!-- Run the map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find my_map_package)/my_map.pgm my_map_resolution"/>

  <!--- Run AMCL -->
  <!--- We load ACML here with diff=true to support our differential drive robot -->
  <include file="$(find amcl)/examples/amcl_diff.launch" />

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find my_robot_name_2dnav)/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_robot_name_2dnav)/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_robot_name_2dnav)/base_local_planner_params.yaml" command="load" />
  </node>
</launch>

You should now be able to run navigation with the default local and global planners. Check the references in the above tutorial in order send navigation goals.

Set up and configure teb_local_planner

After your robot now properly navigates in a (simulated or real) environment, you can activate the teb_local_planner as local planner. Insert the line

<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />

to your move_base.launch file:

<launch>
  <master auto="start"/>

  <!-- Run the map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find my_map_package)/my_map.pgm my_map_resolution"/>

  <!--- Run AMCL -->
  <!--- We load ACML here with diff=true to support our differential drive robot -->
  <include file="$(find amcl)/examples/amcl_diff.launch" />

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find my_robot_name_2dnav)/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_robot_name_2dnav)/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_robot_name_2dnav)/base_local_planner_params.yaml" command="load" />

    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <param name="controller_frequency" value="10.0" />
  </node>
</launch>

Note, we also added a parameter to adjust the controller frequency.

Now update your base_local_planner_params.yaml configuration file with the parameters of the teb_local_planner package. To capture nearby all parameters, use the following template (Version 0.3+):

TebLocalPlannerROS:

 odom_topic: odom
 map_frame: /odom
    
 # Trajectory
  
 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5
    
 # Robot
         
 max_vel_x: 0.4
 max_vel_x_backwards: 0.2
 max_vel_theta: 0.3
 acc_lim_x: 0.5
 acc_lim_theta: 0.5
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "point"
   radius: 0.2 # for type "circular"
   line_start: [-0.3, 0.0] # for type "line"
   line_end: [0.3, 0.0] # for type "line"
   front_offset: 0.2 # for type "two_circles"
   front_radius: 0.2 # for type "two_circles"
   rear_offset: 0.2 # for type "two_circles"
   rear_radius: 0.2 # for type "two_circles"
   vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

 # GoalTolerance
    
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
    
 # Obstacles
    
 min_obstacle_dist: 0.4
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 30
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization
    
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 alternative_time_cost: False # not in use yet

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

Afterwards test your setup by restarting the navigation launch file(s) and commanding a new goal.

Feel free to use rqt_reconfigure in order to adjust supported parameters during runtime:

rosrun rqt_reconfigure rqt_reconfigure

Example Setup

You can find an example setup with the stage simulator in the teb_local_planner_tutorials package.

1. Install the teb_local_planner_tutorials package

2. Inspect the parameter/config files

3. Launch the diff_drive setup: roslaunch teb_local_planner_tutorials robot_diff_drive_in_stage.launch

4. Modify parameters using rosrun rqt_reconfigure rqt_reconfigure or by adapting the files.

Troubleshooting

The resolution and size of the local costmap (see local_costmap_params.yaml) significantly influences the performance of the optimization since each occupied costmap cell is considered as a single point-obstacle. For a huge number of obstacles, parallel planning in distinctive topologies could fail due to numerical instabilities. In that case try to increase the resolution or decrease the size until this problem is fixed or bypassed well (otherwise you could try to slightly reduce the value of h_signature_prescaler).

Known Issues

If the robot tries to rotate in a (really) narrow hallway it might happen that the planned trajectory alternately switches between different motion patterns (left/right/forward/backward) due to noisy laser data, odometry and corrections of the amcl pose. The robot could get stucked (this issue affects mainly carlike robots; for my diff-drive robot the effect is neglectable). If anyone has suggestions, solutions or ideas, please share it on github (report issue/pull request).