Add IsStoppedBTNode - Githubissues

tonynajjar commented 3 days ago

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:

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

tonynajjar commented 3 days ago

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

codecov[bot] commented 3 days ago

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

Files with missing lines	Coverage Δ
...or_tree/plugins/condition/is_stopped_condition.hpp	`100.00% <100.00%> (ø)`
...or_tree/plugins/condition/is_stopped_condition.cpp	`100.00% <100.00%> (ø)`
...havior_tree/plugins/decorator/speed_controller.cpp	`84.78% <ø> (-0.33%)`	:arrow_down:
nav2_util/include/nav2_util/odometry_utils.hpp	`100.00% <100.00%> (ø)`
nav2_util/src/odometry_utils.cpp	`100.00% <100.00%> (ø)`

... and 6 files with indirect coverage changes

tonynajjar commented 2 days ago

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

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 commented 1 day ago

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 https://github.com/ros-navigation/docs.nav2.org/pull/612#issuecomment-2504507376 (I know looking at comments after a PR is merged gets lost in the general "merge" notification, for me at least :wink: ).

tonynajjar commented 1 day ago

@SteveMacenski it's theoretically ready from my side but I'll stress test it even more in the coming week so we can wait a bit before merging

ros-navigation / navigation2