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

Following the Global Plan (Via-Points)

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.

Keywords: Trajectory Optimization Local Planner Navigation Path-Following Via-Points

Tutorial Level: INTERMEDIATE

Next Tutorial: Costmap conversion

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:

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:

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.

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 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):

You might repeat this section with the car-like robot model:

roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch