## 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= 
## descriptive title for the tutorial
## title = Set up and test Optimization
## multi-line description to be displayed in search 
## description = In this tutorial you will learn how to run the trajectory optimization and how to change the underlying parameters in order to setup a custom behavior and performance.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link= [[teb_local_planner/Tutorials/Inspect optimization feedback|Inspect optimization feedback]],
## next.1.link= [[teb_local_planner/Tutorials/Configure and run Robot Navigation|Configure and run Robot Navigation]]
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords = Trajectory Optimization Local Planner Navigation
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Install Package ==

<<Version()>>

{{{#!wiki version hydro_and_older
'''The [[teb_local_planner]] package is not availabe in ROS $ROS_DISTRO.'''
}}}

{{{{#!wiki version indigo_and_newer

Install the [[teb_local_planner]] package from the official ROS repositories.
{{{
sudo apt-get install ros-$ROS_DISTRO-teb-local-planner
}}}

If you build the package from source, make sure to install the dependencies first:
{{{
rosdep install teb_local_planner
}}}

Supplementary material for the following tutorials is available in the [[teb_local_planner_tutorials|teb_local_planner_tutorials]] package.

Check it out from source in order to inspect the files and easily change parameters:
{{{
cd ~/catkin_ws/src
git clone https://github.com/rst-tu-dortmund/teb_local_planner_tutorials.git
# install dependencies, e.g.
sudo apt-get install ros-$ROS_DISTRO-stage-ros
}}}
or install the examples from the official repositories if you just want to run the scripts:
{{{
sudo apt-get install ros-$ROS_DISTRO-teb-local-planner-tutorials
}}}

}}}}

== Configure Optimization ==

The package includes a simple test node (`test_optim_node`) that optimizes a trajectory between a fixed start and goal pose.
We first start configuring the planning of a single trajectory (Timed-Elastic-Band) between start and goal, afterwards we will activate and set up the planning in distinctive topologies.

=== Optimization of a single Trajectory ===

Deactivate parallel planning using the ROS parameter server (make sure to have a `roscore` running):
{{{
rosparam set /test_optim_node/enable_homotopy_class_planning False
}}}

Launch `test_optim_node` in combination with the preconfigured [[rviz]] node for visualization:
{{{
roslaunch teb_local_planner test_optim_node.launch
}}}


A new rviz window should open similar to that shown in the following figure:

{{attachment:rviz_single_teb.png|Visualization: Optimization of a single trajectory|width="750"}}

Three point obstacles are included. They are represented as an [[interactive_markers]] type and therefore the obstacle configuration
can be changed by clicking and holding the blue circle around each individual obstacle:

{{attachment:rviz_single_teb2.png|Visualization: Optimization of a single trajectory in a different obstacle configuration|width="750"}}

Since the Timed-Elastic-Band utilizes a local optimization scheme, the trajectory cannot transit across obstacles. This issue is addressed in the subsequent section.

But first we customize our optimization by running [[rqt_reconfigure]]:
{{{
rosrun rqt_reconfigure rqt_reconfigure
}}}

{{attachment:teb_reconfigure.png|rqt_reconfigure: Change parameters of the optimization at runtime|width="750"}}

Try to customize the optimization according to your desires. But modify the parameters only slightly, since some parameter sets could lead to undesired convergence behavior or a bad performance (especially by changing the optimization parameters).

If you experience a bad performance on your system even with the default setting, try to adjust the following parameters in order to speed-up the optimization:
 *Decrease `no_inner_iterations`
 *Decrease `no_outer_iterations`
 *Increase `dt_ref`
 *Decrease `obstacle_poses_affected`



=== Optimization of multiple Trajectories in distinctive Topologies ===

We now address the problem of local optimization schemes and enable the parallel planning in distinctive topologies.
This extended planner is enabled by default and requires more computational resources.

Restart `roscore` or reactivate the extended planner:
{{{
rosparam set /test_optim_node/enable_homotopy_class_planning True
}}}

Launch `test_optim_node` in combination with the preconfigured [[rviz]] node for visualization:
{{{
roslaunch teb_local_planner test_optim_node.launch
}}}

A new rviz window should open similar to that shown in the following figure:

{{attachment:rviz_multi_teb.png|Visualization: Optimization of multi trajectories|width="750"}}

As in the first section, all obstacles can now be moved using the computer mouse.
The currently best trajectory (in sense of cheapest optimization cost) is highlighted by showing the individual poses (as red arrows) at each trajectory configuration.

Change the obstacle configuration and observe what's happening:

{{attachment:rviz_multi_teb2.png|Visualization: Optimization of multi trajectories in a different obstacle configuration|width="750"}}

Again customize the optimization by running [[rqt_reconfigure]]:
{{{
rosrun rqt_reconfigure rqt_reconfigure
}}}

There exist a separate parameter section for parallel planning in distinctive topologies. Adjust the parameters according to your desires.




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