Use cases

• An agricultural robot that plows, seeds, etc. by going back and forth on a given area.
• A patrolling robot that covers the perimeter of some area. The perimeter to cover (waypoints) are known in advance.
• A warehouse robot that transports goods from one area to another while restricted to follow some route.
• A retail robot that checks specific locations sequentially. Locations are computed at runtime.

We can roughly group these into the following general applications:

• Route / Topological Navigation. We have actually two situations here: a) nodes require the robot to stop and perform an action (possibly outside of navigation), b) robot just moves between points without stopping. Based on our definition (see next section), the former wouldn't be a supported use case. In both cases, waypoints are usually sparsely spaced.
• Local constraints within a global path. In this use case, the robot goes from an initial pose to a final pose with clusters of waypoints.

Definition

We refer to waypoint following/navigation as the capability of navigating from an initial pose, through a set of intermediate points (waypoints) without stopping and ending on a final pose. Waypoints can be sparsely or densely spaced.

Out-of-scope

We won't consider waypoint following applications where segments between waypoints require different behaviors, planners or constraints, or if the robot should stop between waypoints. That feature could be implemented by driving nav2 externally, see option 1.

Requirements

1. User Interaction
   1.1. Users can provide commands through RVIZ or command line. Interfaces for all features are implemented on both.
   1.2. Users can provide a single initial waypoint and start navigation. If more waypoints are passed, the user has the option to select if these are enqueued or preempt existing waypoint.
   1.3. Users can provide multiple waypoints then start navigation. If more waypoints are passed, the user has the option to select if these are enqueued or preempt existing waypoints.
   1.4. Users can pause and resume navigation after it has started.
   1.5. Users can stop waypoint following. This also clears all existing waypoints.
2. Navigation Behavior
   2.1. The robot must not stop at waypoints, only at the final pose.
   2.2. The robot need only have the correct orientation of the final pose.
   2.3. Pausing navigation while navigating to a waypoint causes the robot's motion to stop. Resuming continues motion towards the same waypoint.
3. Settings/Parameters
   3.1. Users can specify what controller or planner is used for waypoint following, possibly picking a different from the ones used for pose-to-pose navigation.
4. Performance
   4.1 Robot should reach each waypoint within a provided tolerance.

Designs

There are multiple ways to implement this, here are some possible options:

1. An external Waypoint Navigator receives the waypoint-following action which is tracking the robot's current position and preempting the NavigateToPose action on BTNavigator in order to get the desired behavior.

• assumptions: We consider pose-to-pose navigation as the core task and more complex tasks (coverage, waypoint following, navigate-to-area, etc.) are achieved by updating the goal pose as needed.
• cons: Can we ensure we stay close to waypoints? Is pose-to-pose really the only primitive?

The idea can be abstracted more to define a general-purpose Mission Executor.

2. A parallel module to BT_Navigator, BT Waypoint Navigator, implements the logic for waypoint following and uses the existing planner and controller action interfaces.

• q: How good does this scale with new navigation tasks? We're prompting developers to create new BT modules for new nav tasks.

3. Use a more general Navigate-through-points primitive. This requires to mainly extend the planner server (as proposed on Navigation following a set of waypoints #1327). All the waypoints are passed to the Planner Server where it computes a path through them. The path is recomputed regularly and waypoints are removed as these are reached. Also, the plan might only be computed over the next waypoint(s). Could be passing a waypoint at a time by adding a field to the action indicating if the pose should be enqueued or preempted.

Proposed Approach

And the winner is... option 1 🥇

The alternatives were discussed at the Navigation WG, agreeing that having an external layer exercising the Nav2toPose action would be best, given that we consider Nav2Pose as the most general primitive.

While options 1 and 2 are very similar, 2 might result in some behavior/code duplication between the different navigators.

Considerations

Tolerance around a waypoint must be above some threshold.

Next steps

Review #1338.