Movebase with Sparse-Waypoint Goals

This package has been derived from the original move_base node from ROS Navigation package. It implements the move_base_swp node, which extends the interface of the the original move_base node to include the sparse waypoints that the robot must visit along its route to the goal.

‘move_base_swp’ is a drop-in replacement for move_base, designed to work with all existing plugins (e.g. DWA planner, navfn, costmap2d) from the Navigation package. The interface is fully compatible with move_base and will accept single-pose goals and behave just as regular move_base with a couple of improvements listed below. The new interface accepts the sparse-waypoint goals which specifies a list of poses, with last pose being the final goal. If a single-waypoint goal is provided the goal reduces to a final goal with no waypoints only issued over the new interface. All other components will work, so move_base_swp is simply a better move_base.

Feature Overview

In this section, we describe the improvements that move_base_swp implements on the top of original move_base node.

Sparse-waypoint goals

move_base_swp implements a new actionlib interface, which allows the caller to define the goal in form of a list of waypoints. Unlike dense waypoints generated by the global planner, this list is sparse. Typical size of this list will include no more than 5-10 waypoints, sometimes just one. The path between sparse waypoints is still within the responsibility of the global planner. In other words, the input to move_base_swp is a list of sparse waypoints that represent an the goal along with a list of interim checkpoints that the robot must visit along its path. The global planner produces a list of dense waypoints in the same manner as in the original move_base, which are fed to the local planner, which, in turn, produces the velocity vector that creates the motion.

While it is theoretically possible to supply a dense set of waypoints over the interface and effectively override the global planner, this is not intended usage. Such usage will likely result in poor performance because of frequent calls to the global planner. The intended usage is for situations when the robot must get from point A to point B, but the application requires that it visits point C along the way. Simply issuing the goal at point B would result in finding the shortest path within the constraints of the cost map which may or may not include point C. If the point C is on a “detour” path it will not be visited. Using move_base_swp allows the application to specify this additional constraint.

While it is possible to achieve the described behavior by first issuing the goal to point C and then as the robot approaches that goal issue the new goal to point B, this will likely result in unwanted slowdown or even complete stop at the interim waypoint. Further, an application would have to constantly monitor the progress of the first goal and issue the next goal “just in time” to ensure smooth transition. Often the “just in time” would imply using ill-defined heuristics that heavily depend on local and global planner tuning parameters.

move_base_swp solves the problem by first calling the global planner for each segment of the path (A-to-C segment, followed by C-to-B segment in our example) and then concatenating the two plans into one global-planner solution before feeding it to the local planner. The result is a smooth motion at the concatenation point without the need for application to intervene in any way. The application simply follows the progress towards the specified goal which includes the specified sparse waypoints.

Windowed global plan

move_base_swp provides an option to feed the horizon of the global plan that is being fed into the local planner. Instead of feeding an entire global plan, move_base_swp has an option to feed only a specified number of plan waypoints starting from the present location and load additional plan waypoints as the robot progresses towards the goal.

This feature can improve motion behavior in general and can stand on its own regardless of whether sparse-waypoint plan is used or not. However, the problem that the windowed global plan solves is exacerbated by the usage of sparse waypoints.

An example of the problem that exists even in the original move_base package is illustrated in this video:

https://youtu.be/7vnxZ25MzEk

The robot is moving from one side to the other of an X-shaped obstacle. Notice how at 0:57 it enters the state where it is not making progress. This happens because the entire area of interest is within the local planner window (we used DWA planner for this experiment). The local planner has the choice to either produce the velocity vector that takes it along the route prescribed by the global plan (the top path shown in green) or to deviate from the global plan and move the robot down the bottom path. From the local planner’s perspective, both paths have equal cost for the set of parameters it is using and the solver is bouncing back and forth between two options without making much progress.

While one motion may eventually prevail, the time it will take for this to happen, is not bounded. Changing the weight factors in the DWA planner to prefer sticking to the global plan more than advancing towards the target (i.e. cutting corners) would resolve this case, but there is always another case for which the same phenomena would happen. Triggering the

File truncated at 100 lines see the full file