Enable Pause/Resume in NavigateThroughPoses & NavigateToPose

[tonynajjar]

Feature request

Feature description

#4994 introduced the concept of WaypointStatus to keep track of the PENDING vs COMPLETED waypoints and that works well for one-shot call to NavigateThroughPoses.
Typically these missions can run for longer and it would be desirable to pause and resume.
This feature request is therefore to implement a pause/resume capability.

Implementation considerations

We started some implementation discussions already here

My custom approach:

Here is roughly how I have it implemented:

- The navigator client (some kind of mission manager) is responsible for keeping track of the status of all the waypoints by subscribing to the feedback of the navigator.
- My NavigateThroughPoses goal sends `WaypointStatus[]` instead of `nav_msgs/Goals` so that we can pass the statuses
- The navigator's internal `waypoint_statuses` is initialized with the ones provided from the client and only the `goals` that are PENDING are set on the blackboard 

Steve's suggestions:

Some other thoughts for a hybrid approach:
- We could have a "ResumeNavigateThroughPoses" and "ResumeNavigateToPose" (with complimentary "Pause" ones) so that we have different semantically meaningful APIs for Start/Cancel/Pause/Resume that each have their contextual information. No need to change the main Action APIs, just have new ones that have that information. I think sharper messages are good to have rather than trying to cram everything into one.  
- The Pause can return the state and the Resume can send it back. 
- Or, the Pause can store that in the navigator itself and the Resume can load it back up in a new tree, so that the application doesn't have to care (but can still return it anyway in case they care or want to persist between reboots. If Resume is given values, it uses those instead of its internally stored one).
- Or, the Pause can simply stop ticking the tree to cold store it and the Resume can continue the ticking loop (TBD: some nodes may care about timing or have progress checking, that would need to be considered. Maybe we just `halt` the tree but keep its blackboard active to resume in a fresh new-tick state? That's essentially what your current implementation and the other ideas above do anyway by resume being a re-goal-submission)

[SteveMacenski]

My response from the other thread: #4994 (comment)

I think these could both make sense. The benefit of the external client is that you can reboot the system (as long as the data is stored to file by the mission manager) and be able to restart a mission. However that comes at the cost of solution complexity and requires the user to take control. The benefit of the internal solution is that its relatively sleek and doesn't involve alot of data being moved around outside of their natural scopes. The drawback being if we need to bring down the server and bring it back up, then that information is lost.

So, I think what makes the most sense is a hybrid approach. We provide Resume and Pause actions that stores that data locally and returns it in the message. If the message is populated then we use the message's content, otherwise we use the locally stored versions. That lets you use it which ever way you like while importantly being able to leverage @tonynajjar's already proven method in the navigator implementation. It also leaves the main Navigation actions untouched with nav2_msgs/Goals which I think is more natural.

I know @tonynajjar mentioned not being able to work on it since they have something internally that works well enough, but how do you feel about that hybrid solution?

[redvinaa]

Hi, me and my team have been using our own solution to pause/resume for quite some time now, it might serve as inspiration.

We implemented a Pause BT control node that wraps the whole tree. It is basically a state machine, and it implements a pause and a resume service. It starts out running the UNPAUSED branch, and halts it on pause, restarts on resume.
To not start everything from scratch each time, we also have a SequenceWithBlackboardMemory node, that stores the index of the last child that succeeded on the blackboard (which doesn't get reset on pause). Likewise, all data that we don't want to be lost on pause (halt), we store on the blackboard.

If you find it useful, we can share the code. Here is an example xml with this setup:

<root BTCPP_format="4" main_tree_to_execute="NavigateToPoseTask">
    <BehaviorTree ID="NavigateToPoseTask">
        <Sequence>
            <Script code="unpaused_branch_child_idx := 0" />
            <Pause pause_service_name="/navigate_to_pose/pause"
                resume_service_name="/navigate_to_pose/resume">

                <!-- UNPAUSED branch -->
                <SequenceWithBlackboardMemory current_child_idx="{unpaused_branch_child_idx}">
                    <!-- First step we don't want to repeat -->

                    <!-- Second super important step -->

                    <!-- Cleanup -->
                </SequenceWithBlackboardMemory>

                <!-- PAUSED branch -->
                <AlwaysSuccess />

                <!-- ON_PAUSE branch -->
                <TriggerEvent service_name="/led_signal/idle" />

                <!-- ON_RESUME branch -->
                <PlaySound service_name="/play_sound" sound="resume" />
            </Pause>
        </Sequence>
    </BehaviorTree>
</root>

[SteveMacenski]

I think pushing a branch to be linked here would be very useful indeed as a reference!

[tonynajjar]

I know @tonynajjar mentioned not being able to work on it since they have something internally that works well enough, but how do you feel about that hybrid solution?

I was already okay with saving the state in the mission manager so also saving the state in the navigator and let the user choose is perfectly fine by me.

[SteveMacenski]

I'm not sure I can put this on my schedule in the immediate future, but happy to help contributors implement and have this included in Nav2 if they're interested and need the feature! Otherwise, with this ticket its on my radar to address at some point down the line.

It sounds like we'd like to go one of two direction:

• Adding a new Pause and Resume actions to the Navigator plugins which would return the waypoint statuses to use but also store them locally. We halt the tree, but keep the state fresh and not delete it (i.e. keep all the blackboard variables around). Then, restart at that time.
• We add a Pause and Resume service inside of a BT node that can be used in any BT XML - that halts the main tree when paused and reticks when resumed.

The main difference here is where the Pause / Resume do their work: inside of the BT or outside of the BT. But either way I think they should both return / accept waypoint statuses like @tonynajjar mentions so that it can be paused or resumed in various states including handling reboots.

[redvinaa]

@SteveMacenski Unfortunately it's not so easy, because we have our custom BT nodes in a separate package. But if it's decided that we're gonna go that way, I can move it to nav2 and open a (draft) PR

[SteveMacenski]

Got it- I think you can hold on until someone's going to work on it then :-) Your time is valuable.

I can see the value prop of both. It 'feels' to me like a Navigator plugin item as a matter of separation of concerns since that is what is exposing the actual action server and handling cancels. Pause / Resume seems like an extension of that & then you don't need to redesign your behavior trees to leverage it.

On the other hand, doing it as a BT means the feature is more flexible. You can add it to any BT you want so it can be added to all navigators instantly, but you'd be at the mercy of proper implementation of the BT using it in order to actually have the feature work as intended.

@redvinaa what are your thoughts? If you were to green field write this again, would you do what you did again or consider the other approach(es) described? I think Tony and I have alignment on the hybrid approach, so if I can get your buy in on that, then I think 3 heads say that's the right direction to go (so others will probably agree and that's the right direction).

Nothing technically stops us from implementing both. If you have something you like and works, we could merge that in :-)

[redvinaa]

I think the Pause BT node is better in the way that you have easy access to the on_pause and on_resume events, so managing state variables is easier. For example we use it to set status leds, publish changes to the current subtask, playing notification sounds.

On the other hand, implementing pause in the navigator(s' parent class?) is nicer because there's less boilerplate in the xml. I also agree with the separation of concerns argument. Though I think these are less important than easily handling those events above, so I vote BT node. The SequenceWithBlackboardMemory BT node is probably useful either way.

I'll create a branch with these implementations.

[SteveMacenski]

Great! I think the only thing I'd want from this in addition to the code is some docs on https://docs.nav2.org/behavior_trees/index.html about how to set this up and the design for pause within a BT XML. Your code snippet above as an example would be a good addition (with some more written context). Then we can link that updated BT docs page to the Migration Guide entry mentioning there's now a BT enabled pause / resume feature. That'll gain some discoverability.

Optionally, if you're OK spending a little more time on it, a tutorial would be another option explaining how to modify a BT to have a pause/resume behavior. You can start with one of the Nav2 defaults (your pick), do the migration, and then show the final tree with the pause/resume behavior