Add support for preemption in actions

[naiveHobo]

Currently, there's no predefined way to handle preemption in rclcpp_actions. When a client sends a new goal when a previous goal is running, the old goal's state is set to ABORTED. This means that there's no way to differentiate between an abort caused by a true failure in the server and an abort caused by a request for preemption.

A client should be able to distinguish between the result codes received on true abort and abort due to preemption.

There are some ongoing discussions on the tickets and PRs listed below:
ros2/rclcpp#1104
ros2/rclcpp#1117
ros2/rcl_interfaces#105
ros-navigation/navigation2#1652

[naiveHobo]

@jacobperron can we move the discussion here?

[jacobperron]

Yes, this is a good place to consolidate discussion. Thanks!

[SteveMacenski]

@naiveHobo @jacobperron any progress on this?

[jacobperron]

@naiveHobo @jacobperron any progress on this?

I don't have bandwidth to work on this, but maybe sometime for G-turtle.

It would be good to get input from some of the original authors of the design document of actions. cc/ @gbiggs @sloretz

[SteveMacenski]

That would be helpful, this represents a pretty substantial bug in navigation2 that we need to resolve far before g-turtle. Ideally, I'd like a proposed solution with an implementation in review before the end of June. I'm also open to getting directly involved here myself but I think I'm probably more useful after the design discussion takes place and helping with the implementations.

[jacobperron]

Patches like ros2/rclcpp#1117 (and the connected PRs) change API and so I don't think adding them to Foxy is likely to happen. I haven't had a chance to take a closer look into the situation, but if it can't be worked around in navigation2 or patched in rclcpp_action in an API (preferably ABI) compatible way for Foxy, then it will have to wait for G-turtle.

[jacobperron]

it will have to wait for G-turtle.

To clarify, I'm proposing we work on this change for G-turtle.

[SteveMacenski]

Ah yes, sorry, I interpreted that as working on it around g-turtle. I'm OK with it being in g-turtle's release.

Though, I think this is a breaking change that is worth making mid-distribution (assuming it doesn't change default behavior). I'd usually agree we shouldn't change it, but this particular issue at least on the nav side is pretty serious and there's no feasible work around. But lets work through the issue first and then we can figure it out later. Because this is LTS is the reason I want this in since this will represent a really serious bug for years to come after its been fixed. If it was non-LTS, I wouldn't care as much since it would go out of scope relatively quickly.

[naiveHobo]

The proposal is to add a preempt branch to the existing goal state machine. This would allow all the existing action servers to keep running without breaking anything but also give the ability for an action server to initiate a preemption when a previous goal is being handled and a new goal arrives. preempt can be implemented the same way as abort with the only difference being the terminal state of the goal handle.

Would this change then be classified as a breaking change? It looks to me that this is just an addition to the existing capabilities.

I'd love your inputs to this design proposal.

[jacobperron]

@naiveHobo Thanks for providing an updated diagram (visuals are useful!).

After more thought, I'm not sure that we want to add the proposed "preempted" state.

Taking a step back, the only difference in the proposed "preempted" state and the "aborted' state is semantics (ie. why did the action server stop the goal?). It appears to me that there could be many reasons that a server stops the execution of a goal, and we certainly can't be expected to capture specific cases for all applications in this general state machine.

In the context of ros-navigation/navigation2#1652, the navigation2 behavior tree relies on the difference of an action goal terminating because it was overridden by a higher priority goal (e.g. a new path from the planner) versus some other reason (e.g. a path could not be found). If it were some other case, I would say that the result message should contain information to communicate the reason for an aborted goal. But, I see how having a layer of abstraction on top of ROS 2 actions complicates things; the simple action server would need to be adapted to communicate the desired terminal state information, regardless of the type of action.

On the other hand, it might be worth adding the new "preempt" goal state to make implementing N-goal action servers (like the simple action server) more straight-forward. I don't think I've thought enough about adapting the simple action server to resolve ros-navigation/navigation2#1652.

Would this change then be classified as a breaking change? It looks to me that this is just an addition to the existing capabilities.

It depends on how it is implemented. One of our goals is to maintain ABI compatibility within a ROS distribution. While we may be able to maintain ABI compatibility, I'm wary about breaking application code that may be relying on the goal state (e.g. what happens when an application receives an unexpected goal state "preempted"?). As a concrete example, the command line tool ros2action should be updated to be aware of new states. Admittedly, it would be a trivial change in the case of ros2action, but I would expect many similar changes to be needed in other application code. IMO,the risk of a more severe break is too great. Perhaps I'm wrong, but we should keep this in mind as we move forward. E.g. it may be the case that we update the state machine during the Galactic cycle and decide the risk of backporting to Foxy is acceptable.

[SteveMacenski]

Small thing, but I think in your diagram the arrows with "preempt" should be "preempting" to be in line with the other ones, but not a big deal.

I don't think that there should be a transition between cancelling and preempt. If its in the middle of canceling, we shouldn't be able to preempt it, that's probably bad form (and also if its being canceled, that's at odds with the semantic meaning of preemption). I think the only state to transition to preempted is from executing.

Taking a step back, the only difference in the proposed "preempted" state and the "aborted' state is semantics (ie. why did the action server stop the goal?).

Just to flip that around a bit, the only difference between "aborted" and "canceled" is also semantics (ei. why did the action stop the goal?). The way it exits is semantically important to represent as you have already identified by separating the concepts of a failure to process and a request to stop processing. Creating a separate concept of stop processing because superseded completes the exit criteria. This would also be usable for not just 1-goal-actions but also N goal actions where you have a limited number of workers to process goals and you want the most recent ones to go at the expense of older ones. When you exit, then its good to know that you didn't fail to process, or request it, but rather something else was more important so it dropped that goal.

If it were some other case, I would say that the result message should contain information to communicate the reason for an aborted goal.

The result code of the action is what we're looking at and what we should look at. If you suggest that we rather include all that same information in the result of the action server, then what's the point at all of having the rclcpp_action::ResultCode that implements every single other return state? What you'd be suggesting would be to completely duplicate this logic. Could that be done? Sure, but that's a really poor solution. Its also not general so it doesn't help the N other single-goal cases where this is important information (and makes any attempt at a generic action server wrapper fall flat).

While preemption has clear uses outside of the 1-goal-at-a-time action servers, ignoring all that for a moment, a super common use of rclcpp_action is for 1-goal-at-a-time tasks. Lets narrow into that a bit; navigate to a position with a physical asset, move a robot arm to a pose for a pick, move a forklift bed up, move a bulldozer bud down, etc. The return semantics for preemption is really important to be handled generally. The suggestion above to have the return codes in the result is non-general and would require then all of these common applications to have to reinvent the existing wheel per-action-message. To me, that defeats the purpose of then using this general rclcpp_actions library and the really smart design decision to have rclcpp_action::ResultCode. The role of a generic action library is to be able to use it to do generic actions. If we cannot enable a really common application of this library generically, rclcpp_action really isn't all that useful.

Anyhow, I think you get my point, don't mean to harp too much on it. I think the other (bad) option would be to remove the rclcpp_action::ResultCode completely then and make it the action interface's problem to enumerate and communicate the valid return codes per action.

[fujitatomoya]

If it were some other case, I would say that the result message should contain information to communicate the reason for an aborted goal.

since there is a clear difference between abort and preempt including use cases, I am not really comfortable with this either. for out robot products, preempt actually happens much more than abort.

[sloretz]

I'm having trouble understanding the motivation for a preempt state. It looks like nav2_behavior_tree::BtActionNode is behavior tree node that sends a goal to an action server, waits for the result of the goal, and sets an appropriate status for the tree. If the action is successful or canceled by another client, it returns success. If the action is aborted by the server, it seems like it would forever get stuck in the BT::NodeStatus::RUNNING state.

As far as I understand the motivation is to enable BtActionNode to assume the action server aborts currently executing goals to make room for new goals, and enable it to do something when that happens to its own goal. It can't make that assumption now because there's no generic way to tell why the server aborted the goal.

After reading ros-navigation/navigation2#1652 , I can't tell what it is the BtActionNode is supposed to do if it could tell its goal was preempted. I doubt getting stuck in the RUNNING state forever is desired. Maybe the motivation is to make the BtActionNode resend the goal to preempt the goal that preempted it? I doubt that's the motivation either because two BtActionNode would get locked prempting each other forever. What does a preempt state enable BtActionNode to do?

Stepping back, I wonder whether BtActionNode should assume action servers implement ROS 1 SimpleActionServer style preemption. What if the action server can process multiple goals at once (probably not true of an action server in the context of navigating a robot)? What if the action server chose to reject new goals until the current one finished? What if the action server chose to queue goals, holding them in state ACCEPTED while in the queue and them moving to EXECUTING when they're done?

---

Just to flip that around a bit, the only difference between "aborted" and "canceled" is also semantics (ei. why did the action stop the goal?).

The result code of the action is what we're looking at and what we should look at. If you suggest that we rather include all that same information in the result of the action server, then what's the point at all of having the rclcpp_action::ResultCode that implements every single other return state?

This is an interesting way to look at it. In the spirit of flipping things around 🙃 , what if instead of adding more states, we removed existing ones?

Say the state machine for a goal only has three states: Accepted, Executing, and Done.

 +--------+          +---------+
 |Accepted+--------->+Executing|
 +--------+          +---------+
      |                   |
      |                   |
      |                   v
      |                +----+
      +--------------->+Done|
                       +----+

Canceling is no longer a state in the state machine. When the goal is Done, the server includes a ResultCode explaining why the goal was moved to Done with some common values defined:

• Success: the server moved the goal to Done because it was completed as asked
• Canceled: the server moved the goal to Done because a client asked to cancel it
• Impossible: the server moved the goal to Done because the goal is no longer achievable as described
• Preempted: The server moved the goal to Done because it chose to execute a newer goal instead
• Timed out: The server moved the goal to Done because it ran out of time to complete it
• Malfunction: The server moved the goal to Done because it became unable to complete goals, maybe it's not getting odometry or the system has run out of memory
• Unknown: The server moved the goal to Done, and it's too hard to explain why

This would enable others to define their own result code values without waiting on the action design to change, such that they can be proved and added to a list of enums in the next release.