## 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= [[teb_local_planner/Tutorials/Configure and run Robot Navigation|Configure and run Robot Navigation]]
## descriptive title for the tutorial
## title = Following the Global Plan (Via-Points)
## multi-line description to be displayed in search
## description = In this tutorial you will learn how to configure the local planner to follow the global plan more strictly. In particular you will learn how to adapt the tradeoff between time-optimality and path-following.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link= [[teb_local_planner/Tutorials/Costmap conversion|Costmap conversion]]
## next.1.link=
## what level user is this tutorial for
## level= IntermediateCategory
## keywords = Trajectory Optimization Local Planner Navigation Path-Following Via-Points
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

This tutorial targets '''version 0.4+''' of the teb_local_planner package.

== Optimization with Via-Points ==

Similar to previous tutorials, we first investigate how the optimizer behaves under the presence of via-points.
For this purpose we start our test node:
{{{
roslaunch teb_local_planner test_optim_node.launch
}}}
In a new terminal window we fire
{{{
rosrun rqt_reconfigure rqt_reconfigure
}}}
in order to comfortably tune the parameters (via-points are deactivated by default).

Now set the value of parameter `global_plan_viapoint_sep` to something positive (e.g. 1.0).
A positive value activates the consideration of via-points. The actual value is not of any interest here, but it is important for robot navigation and explained in the following section.

Bring your rviz window on top and add a via-point between start and goal using the ''Publish Point'' button from the menu (see the following image). 
You need to select an object in order to add a point, but you can simply select an arbitrary point on the grid.

The result should look similar to the following example:

{{attachment:rviz_via_points_weight_1.png|Optimization result (weight_viapoint=1)|width="750"}}


You might notice, that the trajectory is indeed attracted by the via-point, but it does not reach it.
The main reason is that the optimizer both tries to find the minimum-time trajectory (time-optimality) and to simultaneously minimize the distance
to the via-point. Note, we have two conflicting objectives here as long as the via-point is not located directly on the minimum-time trajetory.
The resulting global minimum turns out to be a tradeoff between both objectives. With additional weights in the cost function, the user can now set a desired preference such 
that the global minimum is shifted.

The weight for minizing the distance to via-points is provided by the parameter `weight_viapoint`.
Now increase the `weight_viapoint` to a value higher than 1.0.

The following figure shows the results for a weight of 10.0:

{{attachment:rviz_via_points_weight_10.png|Optimization result (weight_viapoint=10)|width="750"}}


Now you can try to add more via-points and observe what's happening if you adjust `weight_viapoint`.

Note, obstacle avoidance (keeping a minimum distance to obstacles) may also be conflicting with minimizing the distance to via-points. Do not choose too large values of `weight_viapoint` such that the obstacle costs are "negated"/"negligible".

The planner is even suited for much more complex scenarios, e.g. in reaching the goal in minimum time while visiting a set of arbitrarily located via-points (see the following figure).
Of course they are limitations, just play with the test node and some parameter/obstacle/via-point configurations to get a feel for the planning with via-points.


{{attachment:rviz_homotopy_via_points.png|Reaching the goal in minimum time while visiting a set of via-points.)|width="750"}}

Note, if the closest point on the trajectory w.r.t. a via-point is the start or goal pose, then the via-point is ignored (in the current release). Hence, your via-points added before the start or behind the goal are not taken into account in this scenario.
This does not influence the navigation behavior, since the trajectory is initialized according to the global plan in that case.

== Planning in Alternative Topologies ==

While completing the previous section you might notice that the homotopy class planning can still be enabled.
Two different strategies are available depending on the choice of parameter `viapoints_all_candidates`:
 1. if `viapoints_all_candidates` is set to `true`, all generated candidate trajectories are associated with the set of via-points and each corresponding optimizer tries to minimize the distance to them.
 2. if the value is set to `false`, only the candidate trajectory that belongs to the topology (equivalence class) of the global plan is optimized w.r.t. the via-points. All other ones ignore them.
Note, since the ''test node'' does not contain a global reference plan, option 1 is active here independent of the particular parameter value.

Sometimes the user might want to prefer a candidate trajectory for selection (under the presence of multiple alternative solutions) that is located closer to via-points instead of having a shorter transition time (independent of the optimization weights).
By increasing parameter `selection_viapoint_cost_scale` the weight of the via-point cost function (`weight_viapoint`) is further being scaled, but for selecting a trajectory of the candidate set and not optimization.
You can try it by adding a via-point between start and goal and by further moving an obstacle inbetween such that two different trajectories are generated. By changing `selection_viapoint_cost_scale` you should notice, that the selected trajectory (red arrows) switches.



== Navigation: Follow the Global Plan ==

The following steps assume that you are using the navigation setup provided by the
[[teb_local_planner_tutorials|teb_local_planner_tutorials]] package.
However, you can also test everything with your own robot navigation setup, if you have already configured the teb_local_planner.

Close all previous nodes and start the differential drive robot example:
{{{
roslaunch teb_local_planner_tutorials robot_diff_drive_in_stage.launch 
}}}
In a new terminal start rqt_reconfigure:
{{{
rosrun rqt_reconfigure rqt_reconfigure
}}}

According to the optimization example above enable via-points by setting the parameter 
`global_plan_via_point_sep` to a positive value (start with 0.5).

Now drive the robot around (by setting new goals in rviz) and observe what's happening.

The actual value of parameter `global_plan_via_point_sep` defines the minimum separation between two consecutive via-points along the global plan (in meters).
E.g. by setting the value to 0.5, each 0.5m a new via-point is selected from the global plan (as long as the resolution of the global plan is sufficient).
Hence by adjusting the value you can specify whether a coarse or a fine reference path should be taken into account for path-following.

Now set different navigation goals in rviz and play with different values of `global_plan_via_point_sep` and `weight_viapoint`.

Note, a too fine resolution together with a large optimization weight might influence the obstacle avoidance behavior for (dynamic/appearing) obstacles (since the obstacle cost could be negligible in that case).

The following figure depicts a screenshot showing the path-following mode (w.r.t. the global plan):

{{attachment:rviz_navigation_with_via_points2.png|A differential drive robot that strictly follows the global plan)|width="750"}}

You might repeat this section with the car-like robot model:
{{{
roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch 
}}}



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