BTActionNode does not abort

[maxlein]

I am wondering about these lines here:
https://github.com/ros-planning/navigation2/blob/b01810bcf5da97620afa53ec047f54f34c80c3e1/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L221-L223

If the goal is aborted, the result is veiled as it would have never reached the client and thus abort() won't get called:

https://github.com/ros-planning/navigation2/blob/b01810bcf5da97620afa53ec047f54f34c80c3e1/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L160-L165

Maybe someone can explain the logic behind that, I don't get it.
It took me a while to see that, because I was thinking the action service itself is somehow misbehaving on network layer.

If I remove the if clause, everything works as expected when for example the controller is aborting.

[SteveMacenski]

That is a valid point. When I designed this, I was thinking that the action servers only abort when preempted. This check is added because when a server is preempted (like when a controller gets a new path from the global planner at 1hz) the return code is abort.

I think this is a good bug to consider. How do you propose dealing with preemption to also differ from aborted because the server itself decided to fail?

[maxlein]

Well I think the server has to tell why it aborted, otherwise it's just guessing and assuming on the client side.
I would just replace the action result std_msgs/Empty result with something like an error code.

[SteveMacenski]

PR and discussions for implementation are welcome.

[maxlein]

Looking a bit into this and it could be a bigger change than expected.
The templated SimpleActionServer is used everywhere in nav2 with different actions and so different result msgs.
The most minimalistic version I can think of would be to adapt the accept_pending_goal function.
Specifically the abort call here:
https://github.com/ros-planning/navigation2/blob/bc6a41d08dbf1c36c0c4e32b0a32393194d12ffd/nav2_util/include/nav2_util/simple_action_server.hpp#L246-L249

So by customizing this call, one could at least distinguish between a preempted goal and an erroneous server.

Another possibility would be to pass a specific result to the terminate functions.
Then there is no change on the SimpleActionServer required but the action servers all need to pass a specific error code as a result.
https://github.com/ros-planning/navigation2/blob/bc6a41d08dbf1c36c0c4e32b0a32393194d12ffd/nav2_util/include/nav2_util/simple_action_server.hpp#L363-L366

The client has to assume a preemption by checking for an empty result msg though.

[SteveMacenski]

Another possibility would be to pass a specific result to the terminate functions.

What would that solution look like? That actually sounds very reasonable and I've long felt that was necessary. We didn't do it before because we didn't have action server return types before. This PR #1586 adds feedback to all the action servers (which I've been lazy have haven't closed up, I need to do that soon). Having a PR to implement result types is totally reasonable and should be I'd argue should be done. Does that resolve the preemption vs failure conditions for returning the abort type to the Bt nodes? I suppose the status codes could be the same between all messages/action servers. Maybe make a nav2 Status message type that each has their return type of, and the template on the BT node should be able to handle that. Ah, actually, I don't think they would have to all be the same, since the BT action node implementations have the on_success method where they can look at the result without having to push that check in the underlying bt_action_node.

For argument's sake, if we did make them all use the Status and that was relied on, we would just need to document on the website & in the migration guide that all uses of the nav2 behavior tree actions need to make use of a Status result type in the action message definition (it may have more, but must include this).

I'm not sure I understand the first suggestion you mentioned. Can you clarify if you feel if its a better solution than the terminate one? The best that I can tell, this suggests making the accept_pending_goal method virtual for each implementation to deal with, which is probably not a good solution. I'm not sure though exactly what you propose here. Or do you mean "customize this call" by returning not the empty_result() but an actual result? Is that much different than the terminate suggestion? They both would involve including a non-empty result message of the action and having somewhere in there a status specify the reason for returning (actual failure, timeout, preempted, etc). Should we do both?

[SteveMacenski]

@maxlein any update?

[QQting]

Hi all,

I met the same issue in #1683, and I found the ResultCode is ABORTED when not only the controller_server receives a new goal but also the planner_server fails to create a new path.

https://github.com/ros-planning/navigation2/blob/b01810bcf5da97620afa53ec047f54f34c80c3e1/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L221-L223

[maxlein]

Sorry, lot of work atm ...

Or do you mean "customize this call" by returning not the empty_result() but an actual result?

That's what I meant. Just that we only have to adapt at one place and let all the clients as is. It's less work to do and something like a quick fix but without error codes doesn't make sense looking at the nav stack in a more general way.

They both would involve including a non-empty result message of the action and having somewhere in there a status specify the reason for returning (actual failure, timeout, preempted, etc). Should we do both?

We have two options I think:

1. Having one status msg for all nodes

• the default return value of the status msg could be set to "preempted" so that accept_pending_goal stays as it is or check it for the preempted state if not set to preempted by default

2. Having customized status msgs:

• need to make the accept_pending_goal method virtual for each implementation or have a constraint for status msgs that the default value has to be preempted

Personally I don't like to have any assumptions or constraints one has to look for but depends if it could save a lot of refactoring.

As an example, if we have just one status msg, we would need to change only:

https://github.com/ros-planning/navigation2/blob/6fb43d4a0543897d29d38b3e4a749b6bea6108e1/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L221

to check for the preempted state.
And the nodes which want to handle error states can implement on_aborted().

In my test I have a NavigationResult.msg which looks like that for the moment:

int8 NO_ERROR = 0  or preempted
int8 GENERIC_ERROR = 1
int8 BLOCKED_BY_OBSTACLE_ERROR = 2

int8 status

The question is also if there is a need for customized result states, or if we just have a bunch of states in there? And if someone needs an extra state, add it and make a PR or build nav2_msgs from source.

[SteveMacenski]

I'm a little unplugged from this issue for the moment. Forgive me if below is obvious.

The ending take a result (e.g. terminate_current, terminate_all, succeeded_current, accept_pending_goal), except accept pending goal, with default parameters that no one overrides. All each server would need to do is send their own result and fill it in with whatever context is required to notify about "why" it ended. The BT action node could have an is_valid_result() method that takes the message and returns a bool about "yes, this is an ending that should be processed" or "no, this is a preemption, don't exit". That would replace the ABORTED check.

That would be general and minimal changes required

• Accept pending goal takes a result type for preemption
• Each server's terminal conditions need to be given a result
• Each BT node needs a is_valid_result() override to parse the specific status messages (functionally for our nodes, we could use a nav2_msgs/Status for reducing duplication, but we've made no requirement for it)

I imagine this status just says something like: FAILED, PREEMPTED, SUCCEEDED, CANCELLED (pulling from ROS1 http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html). different from your suggested GENERIC_ERROR, etc. I think these convey the message well.

It's not immediately clear to me why it appears to work for the controller, though. I see in the controller server when the algorithm fails it does a terminate_current and you can easily show in simulation that when the controller fails, it successfully exits and causes recoveries to trigger. @QQting reported that the planner doesn't do that but also terminate_current on failing to compute. @maxlein your custom node (?) also shows that to cause issues. I agree that is should cause issues, so why is the controller working?

That makes me wonder why there is no rclcpp_action preemption distinction, I filed a ticket for that in rclcpp and we'll see what they say (ros2/rclcpp#1104). That's the "best" solution in my mind, because then we just switch the ABORTED to PREEMPTED in the lambda and we're done. All of the other result codes are done with this wrapped result analog to ROS1 actionlib except the preemption code, which is what we really want here. We don't want to have a nav2_msgs/Status that essentially just mirrors that ResultCode if we can help it.

Edit: On your comment about removing the if (result.code != rclcpp_action::ResultCode::ABORTED), is there anything not working by removing that? I recall adding that because the controller would stop without that check. I added that so the goal_result_available_ = true; wasn't called when the controller was still running because of a preemption, causing the controller Bt node to return.

[maxlein]

That makes me wonder why there is no rclcpp_action preemption distinction, I filed a ticket for that in rclcpp and we'll see what they say (ros2/rclcpp#1104). That's the "best" solution in my mind, because then we just switch the ABORTED to PREEMPTED in the lambda and we're done.

Good point, didn't look that deep. That's the way to go.
But what to do now? As I understand this, ideally we would have the action server "layer" with its action status msg (Canceled, Aborted, Preempted, ..) and on top of that the app "layer" with it's result msg (e.g. couldn't plan, couldn't move my forklift to N inches, etc).
So to also support error codes right now, we would need to have both msgs in the result msg:

int8 PREEMPTED = 0  
int8 ABORTED = 1
int8 SUCCESS = 2
...  

int8 NO_ERROR = 0
int8 COULD_NOT_PLAN = 1  
int8 OBSTACLE_DETECTED = 2
...

int8 action_status
int8 error_code

Edit: On your comment about removing the if (result.code != rclcpp_action::ResultCode::ABORTED), is there anything not working by removing that?

Yes, preemptions are not working ;). But in my case it can't happen so this is not an issue for my "custom" controller right now.

[SteveMacenski]

I think the path forward is

• Someone interested in this (hoping @maxlein or @QQting or someone from the summer sprint. Me, maybe if necessary) would be willing to implement the proposed changes to rclcpp_actions and submit a PR. Things are much more likely to get traction with a PR.
• Update our action server wrapper logic to use this preemption method and the BT action node to look for that result code to ignore only.
• Consider what you mention here additional error codes in a nav2_msgs/Status to give more contextual information about why an action failed. This seems action specific so maybe a standard Status message doesn't make sense and its just baked into the action definition.

The alternative to modifying rclcpp_actions is to:

• Create the nav2_msgs/Status message with the same information as rclcpp_action ResultCode + preempted.
• Add to each server action result
• In each server, when exiting, give it that result object with the right code
• Add BT action node is_result_valid() virtual function and have that replace the check for ABORTED in the lambda
• Override the is_result_valid() for each specific BT action now (only a few) to check the code if its preempted, return false, else return true.
• Consider what you mention here additional error codes in a nav2_msgs/Status to give more contextual information about why an action failed. This seems action specific so maybe a standard Status message doesn't make sense and its just baked into the action definition.

Either way, your mentioning of having the result message tell the caller why it failed is valuable. If you're open to submitting PR to implement that for the planner / controller, that would be valuable. On its own, the BT navigator would ignore them, but at least they're there for use later or if called by other clients.

The alternative is obviously less ideal since we have redundant information in the ResultCode from the action server and the result statuses. Also adds more complexity for the BT designers. Also isn't a generic fix. But the +1 is that its all in our repos we control and can push through.

Thoughts?

[QQting]

Hi all,

Here #1683 (comment) is what I've tested.

Currently, I don't have any ideas about how to improve this because I'm not familiar enough about rclcpp_action.

By the way,

Edit: On your comment about removing the if (result.code != rclcpp_action::ResultCode::ABORTED), is there anything not working by removing that?

Yes, preemptions are not working ;). But in my case it can't happen so this is not an issue for my "custom" controller right now.

Is it the reason my global plan cannot avoid a dynamic obstacle because preemptions are not working?