Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Set up and test Optimization
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.Keywords: Trajectory Optimization Local Planner Navigation
Tutorial Level: INTERMEDIATE
Next Tutorial: Inspect optimization feedback, Configure and run Robot Navigation
Contents
Install Package
Show EOL distros:
The teb_local_planner package is not availabe in ROS $ROS_DISTRO.
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 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:
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:
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
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:
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:
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.