Add IsStoppedBTNode

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)

---

Description of contribution in a few bullet points

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.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

[tonynajjar]

@SteveMacenski are you open to this contribution? Still a draft but almost there

[SteveMacenski]

Groot (https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/nav2_tree_nodes.xml) and doc updates are needed (the usual bits!).

[codecov]

Codecov Report

Attention: Patch coverage is 89.36170% with 5 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_util/include/nav2_util/odometry_utils.hpp	76.47%	4 Missing ⚠️
...or_tree/plugins/condition/is_stopped_condition.cpp	95.83%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...or_tree/plugins/condition/is_stopped_condition.hpp	100.00% <100.00%> (ø)
...havior_tree/plugins/decorator/speed_controller.cpp	84.78% <ø> (-0.33%)	⬇️
nav2_util/src/odometry_utils.cpp	100.00% <100.00%> (ø)
...or_tree/plugins/condition/is_stopped_condition.cpp	95.83% <95.83%> (ø)
nav2_util/include/nav2_util/odometry_utils.hpp	76.47% <76.47%> (-23.53%)	⬇️

... and 6 files with indirect coverage changes

[SteveMacenski]

On terminal conditions (success, fail) shouldn't we update stopped_stamp_?

[tonynajjar]

On terminal conditions (success, fail) shouldn't we update stopped_stamp_?

With "update" I assume you mean resetting to rclcpp::Time(0, 0, RCL_ROS_TIME)? For FAILURE we do. For doing it at SUCCESS, I think it's a tradeoff:

• reset at SUCCESS: Not stateful: Next time it's ticked, it will wait again for duration_stopped, which could be undesired or unexpected behavior. But maybe stateful nodes is an anti-pattern so this behavior is actually expected?
• no reset at SUCCESS: Stateful: Next time it's ticked, it won't wait "since it's been stopped for long enough already". However there could be this bug:

           1. BT node returns SUCCESS after it was stopped for long enough
           2. robot moves and stops again before the BT node is ticked again
           3. BT node returns SUCCESS because the moving in between was not considered

All things considered, I think you're right that resetting at success would be the better option

[SteveMacenski]

Agreed, I was thinking something too like node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(duration_stopped_) will always be true if we left the branch of the BT that contains this node and then come back to it after some time, if we don't reset the time after terminal conditions. Then, its a single-shot of the velocity without using your duration piece.

With that said, I think it will wait again for duration_stopped, which could be undesired or unexpected behavior can be largely mitigated if you have your waiting duration to be sub-100ms -- about the perception of a person to change. 100ms though should give you some readings to work based off of so any single erroneous measurement doesn't get you.

---

Also see ros-navigation/docs.nav2.org#612 (comment) (I know looking at comments after a PR is merged gets lost in the general "merge" notification, for me at least 😉 ).

[SteveMacenski]

LGTM - let me know if there's anything else here from that conversation of its good to go

Though, I see a couple of lines that seem easy to cover in unit testing that are not covered:

• twist.header.stamp = node_->get_clock()->now();
• Each of the !received_odom_ condition's internals

https://app.codecov.io/gh/ros-navigation/navigation2/pull/4764?dropdown=coverage&src=pr&el=h1&utm_medium=referral&utm_source=github&utm_content=checks&utm_campaign=pr+comments&utm_term=ros-navigation