bt_service_node and bt_action_node: Don't block BT loop

[christophfroehlich]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	none
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo classic simulation of inhouse robots
Does this PR contain AI generated software?	no

---

Description of contribution in a few bullet points

• Since Log if BT rate is exceeded #3909, we get console warnings if a service call takes more time than a BT loop duration.
• The timeout for spin_until_future_complete introduced with Use incremental timeouts in spin_until_future_calls during BT execution to handle large server timeouts #2320 waits exactly bt_loop_duration_, which means that loopRate.sleep() might return false if anything else needs more time in the BT to finish the loop.
• I changed the spin_until_future_complete to be non-blocking, which works fine in our setups.

Description of documentation updates required from your changes

none

---

Future work that may be required in bullet points

Please backport also to iron and humble if possible.

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@christophfroehlich, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@christophfroehlich, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

Maybe we've hit the edges of my knowledge on spin_until_future_complete, but I can't find any uses of it with the timeout set to 0 time. Are you sure that this works as expected? Looking at tests in rclcpp, I can see uses of it with 1ms, but never less than that value. I could see the implementation of the spin function looking at the time, and if expired, returning before ever trying anything. If set to 0, it would trigger that action. It would be dependent on the implementation that could be subject to change.

So, instead I think we should make this non-zero (so we can wait a bit to get an immediate response if possible) but your point has been made that waiting the full timeout will keep the BT ticking at approximately the set rate, but occasionally violate. Perhaps we should make this 1ms (which would limit the tree's tick rate to 100hz) or some percentage of the total tick rate (i.e. 50%, 80%, 90%).

Also, I think this exact same logic should be done in the bt_action_node, for which this service node was based off of

[christophfroehlich]

I tested it for some days now within our setup, and I haven't seen any problems. If the service result is not there, it resumes with the tick. Otherwise it takes the result.

I don't know the rclcpp code very well, I just have seen the API docs and thought I could give the non-blocking option a try

• \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
• -1 is block forever, 0 is non-blocking.

[SteveMacenski]

I'm less concerned about today and more concerned about tomorrow. Right, it would be easy for a contributor to rclcpp to change something from:

• Check timeout -> do thing
• Do thing -> check timeout if continue

and seemingly both are correct, If we have a timeout of 0. We're a bit at the mercy about how that implementation works today (or how the RMW layer implements it perhaps even). Rather if we set a logical timeout, then we negate this problem and use more of the tick time to actually get a result to be slightly more reactive than we would be otherwise. So, my preference would be to not set this to 0 for future proofing purposes (even if it works today) knowing that we're relying on a gray area in doing so. Though obviously I'm worried about it working today, but I take your experience as fact that it works fine.

Does that make sense? We could just add a multiplier on the BT loop time (0.5 - 0.9) so that we use less than the loop time for waiting on the service / action so we meet out timeout deadlines, but have more time waiting to be immediately reactive with results to move on in the system. I figure ~half is a good metric

[christophfroehlich]

I understand your concerns, that makes sense to me. I still hope that such behaviour-braking changes won't get merged silently in core packages ;)

I'll test your proposal and will report back

[SteveMacenski]

I still hope that such behaviour-braking changes won't get merged silently in core packages ;)

Same, but I noticed that there are no tests using 0ms 😉 So it would be forgivable

[mergify]

@christophfroehlich, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@christophfroehlich, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@christophfroehlich, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

Can this be expanded to the BT action node as well? They have the same characteristics and I'd be surprised if for some reason that didn't cause you to lose loop rate as well

[christophfroehlich]

I never had problems with the action node.

However, I tried the same already in the following section
https://github.com/ros-planning/navigation2/blob/a1997db142bef4aff898ab72fc250adba09825e6/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L414-L416
but this failed consistently with the action getting cancelled immediately.

[SteveMacenski]

Maybe there was a bug in the service node then? That's the same design pattern in both (literally I copy + pasted it over and modified for services) so I don't know why services would sometimes exceed but actions do not. Can you look into some potential mishandling of the timeout in the service node before we merge this in? Perhaps there's some error that resolves this instead of setting a fraction

Also, heed the DCO and linting failures, those must be fixed before merge

[mergify]

@christophfroehlich, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[christophfroehlich]

I investigated this now: I introduced an error with the factor, the operator * on std::chrono::milliseconds doesn't do what I expected: the duration is zero afterwards. This worked for the service node (as was my first proposal), but this did not work with the action server.

The difference is that the service node waits with spin_until_future_complete for the result (which can take a lot of time as in my case), but the action node waits with spin_until_future_complete until the goal is accepted. This is maybe why I don't get any loop-duration warnings with the action node.

An explicit cast does the job:

max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);

This works now with both nodes.

[mergify]

@christophfroehlich, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[huemerj]

I've tested the changes, LGTM so far.

[SteveMacenski]

Makes sense, glad I inquired!

[christophfroehlich]

How do you handle backports in nav2? Is it possible to backport this up to humble?

[SteveMacenski]

Yes it can be, I'll handle it next sync. We're in a weird state right now with CI and such so things are a bit unpredictable. It'll get in there though first opportunity

[christophfroehlich]

Yes it can be, I'll handle it next sync. We're in a weird state right now with CI and such so things are a bit unpredictable. It'll get in there though first opportunity

@SteveMacenski May I ping you again and ask for a backport?

[SteveMacenski]

If it wasn't backported (which it looks like it could have been?) it was due to huge merge conflicts on cherry picking. If you'd like this in iron, please submit a PR against iron making these changes manually and I can merge that and have it released in the next sync.

Sorry about the manual-ness, the automated tools must have failed badly enough that I judged it not possible at the time.

[christophfroehlich]

I cherry-picked this one and the other two PRs on top of the humble branch, without big issues. Would you consider them if I submit manual backports for iron and humble? (I'm stuck to humble in an industry project..)

[SteveMacenski]

The main rules are that:

• It cannot modify the meaning of an existing parameter or interface
• It cannot modify default existing behavior, behavior should remain consistent within a distribution update
• It cannot modify API, including function signatures, ROS APIs like interfaces, etc
• It must be largely additive
• It must have sufficient time soaking in main before backport for stability
• It must not depend on previous changes that were not backward compatible

If it meets these, then its a candidate for backporting