## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= [[teb_local_planner/Tutorials/Setup and test Optimization|Set up and test Optimization]]
## note.1= [[navigation/Tutorials/RobotSetup|Navigation: RobotSetup]]
## descriptive title for the tutorial
## title = Configure and run Robot Navigation
## multi-line description to be displayed in search 
## description = In this tutorial you will learn how to set up the teb_local_planner as local planner plugin for the navigation stack.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[teb_local_planner/Tutorials/Obstacle Avoidance and Robot Footprint Model|Obstacle Avoidance and Robot Footprint Model]]
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords = Trajectory Optimization Local Planner Navigation
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Set up your Robot for 2D Navigation ==

Please make sure to setup your robot to comply with the 2d [[http://wiki.ros.org/navigation|navigation]] package (sensors, costmap2d and robot driver/simulator).
Refer to the following tutorial: [[navigation/Tutorials/RobotSetup|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:
{{{
#!block=move_base
<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:

{{{
#!block=move_base_with_teb
<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+'''):

{{{
#!block=base_local_planner_params
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|teb_local_planner_tutorials]] package. 

 1. Install the [[teb_local_planner_tutorials|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).




## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE