Implement waypoint following

[orduno]

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 #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.

[crdelsey]

What's the intended use case?

[bpwilcox]

This is starting to sound like mission execution, in which there may be multiple actions (not only navigateToPose) which are enqueued for the robot to implement. Perhaps this proposed mechanism should be generic so as to support many action types.

[SteveMacenski]

This seems like an application level thing, I wouldn't modify lower level code to support but add a new thing on top that uses the action interface to complete waypoint following

[orduno]

What's the intended use case?

@crdelsey Going back to our work on requirements, one way to think of waypoint following is as a navigation primitive, much like navigate to pose or navigate to an area we would have navigate waypoints. However, I do see this is debatable, one could think of navigating waypoints as a mission planning / execution capability where it uses the navigate to pose primitive and the goal is updated before reaching an intermediate waypoint. I see pros and cons on both of these ideas...

Regardless of how we implement it, I think that waypoint following will help navigate through environments with traffic rules, road descriptions, etc.

[orduno]

This is starting to sound like mission execution, in which there may be multiple actions (not only navigateToPose) which are enqueued for the robot to implement.

@bpwilcox Certainly, as mentioned above, we currently have the navigateToPose primitive, navigateWaypoints could be another for mission execution to use.

Perhaps this proposed mechanism should be generic so as to support many action types.

Certainly, I think the changes needed between different navigation primitives happen on BtNavigator and the corresponding bt_tree and bt_nodes. So no changes at the planner and controller.

[orduno]

This seems like an application level thing, I wouldn't modify lower level code to support but add a new thing on top that uses the action interface to complete waypoint following

@SteveMacenski Another way to see it is as a lateral implementation to navigate to pose, with changes to navigation behavior tree and no changes to lower layers like planner and controller. For now, this is not a high priority but I'd like us to eventually discuss the pros and cons of both approaches.

[orduno]

Closing feature request for now, can reopen once we get to work with MIssion Planning/Execution.

[SteveMacenski]

I think this should be reopened. This is a good basic example use-case of the navigation action. Having an example of how to go from “point and shoot” navigation from rviz to creating an application would be useful.

Im not sure a mission executor or anything behavior tree is needed. Something like read a yaml file of pts, call the bt nav action server, move to next or handle basic timeout/failure cases

[orduno]

OK, will reopen per Steve's suggestion.

[fmrico]

Hi @orduno and @SteveMacenski ,

I have been working on this issue, and I have created a PR #1327 I . I recorded this Video with the result. I would like to know if you consider this approach valid before continuing with it.

I changed the actions ComputePathToPose to ComputePathToPoses, and NavigateToPose to NavigateToPoses. The request is a nav_msgs/Path instead of geometry_msgs/PoseStamped. The idea is making this action more generally. For single pose navigation, send a path with only one pose. For multi waypoint navigation, fill the request with more poses.

I have also added a button to the RViz plugin. It changes to a "recollecting" state, where you can set several waypoints. When finished, push "start navigation" to call the action and return to the idle state.

I preferred this approach instead of creating new actions. This required to duplicate too much code.

I hope it helps!! Francisco

[orduno]

Francisco, thanks for your PR! I've reviewed your approach and provided feedback.

@SteveMacenski @bpwilcox @crdelsey @fmrico: Can we discuss a bit more about alternate solutions to waypoint navigation? Here are some:

1. Mission Executor: The waypoint-following action is received by some Mission Executor, which is tracking the robot's current position and preempting the NavigateToPose action on BTNavigator in order to get the desired behavior. With this approach, we think of nav2 as doing point to point navigation. Complex tasks (coverage, waypoint following, navigate to area, etc.) are achieved by updating the goal pose as needed.
2. Bt Waypoint Navigator: Two options here: 2.1. Add a field to NavigateToPoses.action indicating if the provided pose should be enqueued or preempted. 2.2. Create a new NavigateWaypoints.action, then BTNavigator implements the logic without changes to the planner and controller. We would implement in a similar way other navigation tasks.
3. Waypoint Planner Server: As proposed on #1327, all waypoints are passed to the Planner Server where it computes a path through the waypoints. Here we could compute a plan through all the waypoints at each cycle and pass it to the controller, or just to the next (few) waypoints.

What are your thoughts?

Two other things to clarify:

• Should we differentiate between NavigateToPoses and NavigateWaypoints? To me, the former might require the robot's orientation to match the waypoint orientation whereas on the latter only the final goal pose is required. In the former case instead of using the SimpleActionServer we would use a QueuedActionServer.
• Should we support providing a single waypoint at a time instead of all upfront? In this case the robot should start moving through the available waypoints, as new waypoints come in, these should not preempt the existing one but queued.

[bpwilcox]

I was thinking about some of these issues as I briefly looked at the recent PR. I think there should be a clear and intentional difference between pre-empting a current goal and adding a new one to a queue. I also agree that there is a difference between a waypoint and a pose in that the former doesn't require that the orientation match, necessarily.

I'd like to see good synergy in the goal poses/waypoints such that:

• a user should be able to interrupt the current goal pose/waypoint
• a goal pose/waypoint can be enqueued to be completed next.
• allow goal poses/waypoints to be set up in advance

I believe the an intuitive UI got rviz would be an interesting challene, though.

[SteveMacenski]

I’m still in China for another few days. Can we discuss this in 2 weeks at the weekly?

Edit: I skipped something to think about this. See review in PR.

[fmrico]

Hi @orduno,

As I commented in the PR, I will change the approach. Let me try a new approach using the first two options you suggested (Mission Executor and Bt Waypoint Navigator).

Thanks!!

[SteveMacenski]

I’m not sure I agree with any of those lines of thought at the moment, but I’m also open to changing my mind. There’s 1,000,000 ways to do this. I’m just drawing from how I’ve done it in the past and that’s not necessarily the best way.

I do generally like the idea that navigation is “stupid”, it knows its current task but shouldn’t know about future ones until an application level process gives it to them. However there are probably cases where you want to give N intermediate points to the planner to optimize through, but exposing that at this API level would make basic uses much harder to understand.

Plus I don’t know of any open source planners that could make use of that. If that was exposed I’d make it conditional on an open source client example using it.

[orduno]

I’m not sure I agree with any of those lines of thought at the moment

I thought I was covering your application-level approach in Mission Executor. Could you edit the ticket and provide some high-level info.

There’s 1,000,000 ways to do this

Let's identify some reasonable approaches and discuss these, maybe there are only just a few and the rest are only variations. Please edit the ticket and add any other you think we should consider.

[SteveMacenski]

I think there are (at least) 2 major schools of thought here. One being navigation gets a set of waypoints to plan through and control to, other being navigation gets 1 waypoint fed to it at a time, and on completion you dish out the next.

If there’s no planner provided that can plan through intermediate waypoints (which we dont), theyre essentially the same outcome except you push the behavioral questions to the navigator (ei what to do on failure, stuck, etc of intermediate points). Not good or bad, just is. It depends if you see what as a benefit or a complexity. I see it as a complexity, but only because we don’t have a planner in here that could make use of intermediate waypoints in a continuous, smooth way. This is something often done in manipulation but not by the existing navigation architecture, I haven’t actually heard that request from anyone, but it is a missing capability. Though adding that would require a number of substantial changes so I’d avoid that line of thought until there was a concrete customer.

So really the question becomes whether you want navigation to handle multiple piecewise navigation tasks or a higher level application that does it and decides for itself what to do on failure. He’s done the former, my suggestion is the latter, but neither are inherently better.

[mlherd]

Here is a great example of using waypoints and mission executor idea. Video

Let's say I want to do a very similar application with a rover robot on the ground using Navigation2. Depending on the application, this problem can be more complex or simpler and there are many different ways to solve it. In my opinion, pretty much all these tasks shown in the video and more can be done by using what we currently have in the stack, point to point navigation.

In general, I like the idea of supporting waypoints, but depending on the application type, the user may want different navigation behaviors between and at each waypoint. That is why, I think higher-level applications such as Mission Planner and Mission Executer should handle how to collect a list of goal poses, how to pass them to Nav2, and how to get to the waypoints in terms of what planner to use, and what to do at each waypoint etc.

[mkhansenbot]

We discussed in the Nav2 WG and decided to go with approach 1) above, which is implemented in #1338.

Thank you everyone who contributed to the conversation.