Draft: Added GoalUpdatedController BT plugin node

[relffok]

Signed-off-by: relffok [email protected]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3037
Primary OS tested on	Ubuntu
Robotic platform tested on	turtlebot (simulation)

---

Description of contribution in a few bullet points

• Added a draft of the GoalUpdatedController plugin node

Description of documentation updates required from your changes

• Plugin needs to be listed under Decorator Plugin List

---

Future work that may be required in bullet points

• Tests missing
• Documentation missing

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]

@relffok, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• 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

[relffok]

@SteveMacenski, is this how you had the GoalUpdatedController in mind?

[SteveMacenski]

@naiveHobo mind reviewing this?

[SteveMacenski]

Couple of things off the bat - there's no reason you should know these things so no worries

• You need to find all yaml files in Nav2 and add nav2_goal_updated_controller_bt_node to them, just so anything can use a Bt including this in the future.
• Within BT Navigator, there's a default set of library names, add nav2_goal_updated_controller_bt_node to it too so that if nothing is specified, it has everything in Nav2 in it
• The BT and API needs to be added https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/nav2_tree_nodes.xml so it can be used in Groot
• We require test coverage for all new features in Nav2, see other BT node Prs to see examples of tests https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/test/plugins. We need to make sure that it triggers OK when starting, not triggering when not having updated goals, and triggering when there is an updated goal. This should actually be an unusually straight forward test.

Ignoring the new server code, this PR is a good example of where all these things reside / examples: #2904

[SteveMacenski]

I don't think first_time_ is required, if you place the 2 initial gets inside of the if IDLE condition above. I'm not sure (?) that first_time_ is required in the main if in L65.

Thinking through the logic, if this is called a 2nd time, the goal_was_updated_ will be true. If the child action is still continuing, it'll tick again. So its really just the first time we need to specifically say to go for it.

We could just in if (status() == BT::NodeStatus::IDLE) { say goal_was_updated_ = true since its updated from the idle state reference. That would be the same logic without the first_time_ variable.

[naiveHobo]

I agree with this, first_time_ isn't required.

Although, does it make sense for this node to tick its child when it's updated from IDLE state to RUNNING state on the first tick? The node is only meant to tick its child when goal is updated on the blackboard. But with this logic, we assume that the decorator should tick its child on the first tick and any further ticks where the goal is updated. This logic made sense for RateController and DistanceController where we start the logic at 0 seconds or 0 meters traveled but I'm not sure if GoalUpdated condition requires a mandatory execution of its child on its first tick, that should happen automatically on the first tick when if (goal_ != current_goal || goals_ != current_goals) condition is true.

We could just remove first_time_ and place to 2 initial gets inside the if (status() == BT::NodeStatus::IDLE) condition while setting goal_was_updated_ = false within the condition and leave everything else as it is. That would mean that this node would tick its child only when the goal is updated on the blackboard.

[relffok]

1. Fully agree with the first_time parameter, I wanted to keep it to make it obvious but since the code is so easy, it's quickly figured out.

2. I think from the naming it makes sense not to tick the child on the first visit, I am just wondering if application-wise it makes sense to tick it anyway for the first time. Either way, it's a quick change.

[SteveMacenski]

We should always tick on the first visit for this kind of thing - or else when you submitted a navigation request, it would never plan at all (you'd have to issue another new goal afterwards)

[naiveHobo]

Logically, everything seems alright to me. Just looking for a BT node test to make sure the node actually works as intended.

[relffok]

Updated PR with all the comments, I am not quite sure about the test though.
Is it necessary to check the tick conditions only or also the different status returns of the child node? (I implemented that by setting the status to sucess and false).

It needs to be decided if the child should be ticket when visiting the first time or not. If not, I need to adapt (more of a note to myself):

• node: the goal_updated value to false
• example: navigate_w_replanning_only_if_goal_is_updated.xml logic
• test: status for first tick

[SteveMacenski]

LGTM! Thanks for the contribution! That came through surprisingly fast, good job! I'd welcome your help with any bug/feature in the future!

[relffok]

@SteveMacenski Will this be backported to galactic since it doesn't break anything? Do you do backports in complete sweeps or can I open a PR for that?

[SteveMacenski]

Yes! It'll be backported next time I do a sync to galactic, I do them in batches. But if you're building Galactic from source and want it sooner, we can backport it more immediately

[relffok]

Yes! It'll be backported next time I do a sync to galactic, I do them in batches. But if you're building Galactic from source and want it sooner, we can backport it more immediately

No need but thanks for doing it right away anyway! :)