Adapt GoalUpdater to update goals as well

[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 as usual, still a draft but what do you think of the idea? Do you have a preference for a standalone node? GoalUpdatedCondition checks both goal and goals in the same node.

[codecov]

Codecov Report

Attention: Patch coverage is 92.85714% with 2 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...avior_tree/plugins/decorator/goal_updater_node.cpp	92.00%	2 Missing ⚠️

Files with missing lines	Coverage Δ
...avior_tree/plugins/decorator/goal_updater_node.hpp	100.00% <100.00%> (ø)
...avior_tree/plugins/decorator/goal_updater_node.cpp	95.74% <92.00%> (-4.26%)	⬇️

... and 2 files with indirect coverage changes

[SteveMacenski]

I know its a conversational draft, so obviously the main block in tick still needs the goals version.

I'm not opposed to having this BT node be goal(s). I'm less certain about the introduction of a new PosesStamped message type used only for this application. I imagine the context of use would be for NavigateThroughPoses or similar, so shouldn't those be updated as well to use that [1] (among possibly others)?

Also, food for thought, this seems like a good geometry_msgs improvement rather than a nav2_msgs improvement to add PosesStamped. Maybe we should file a ticket over there and ask @tfoote if he's open to that contribution

[1] https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/NavigateThroughPoses.action#L3

[tonynajjar]

What timestamp-checking logic would you have here? In this implementation I'm just checking the top-level timestamp. We could also check all the individual goals timestamps and reject the update if at least one is outdated

[SteveMacenski]

I think that we should check if any are newer. That would indicate that a new message has come in that was not previously known.

I read this check as looking at the topic's goal and the input port's goal and selecting the topic's if its newer (and a topic update is present). So, I think the analog here would be to check the goals list's header.

I know in the geometry_msgs/PoseStampedArray PR we discussed removing it because we couldn't think of a good reason to include it -- I think this is the good reason to include it. You could have that message type defining a set of goals so the goals themselves have their own stamps as dynamic constraints, yet the global header is referring to the time the goals themselves were computed.

Sorry to flip flop :( To save you a step, I think the header's description that Michael was asking for should be along the lines of saying that the global header is the Time in which the set of pose-stamps were computed, and the posestamped-stamps are the stamps of the poses themselves, which could vary.

From a set of N goals, I agree its challenging to identify if the input or the topic goal list should be used. I think we could do it if any individual goal has a newer timestamp, but that doesn't seem bulletproof

[tonynajjar]

I did not get what you are proposing to compare exactly? Is it if any of the topic PoseStamped are more recent than any of the input PoseStamped? Or if any of the topic PoseStamped are more recent than the stamp of input PoseStampedArray?

If we are keeping the top-level header with the meaning "the time the goals themselves were computed", then I don't see why we would not just keep it as is (compare current and incoming top-level stamps).

[SteveMacenski]

I'm saying that I think we should do the same comparison as the singular goal case; Check the global header only to make sure its a newer computed goal than the previous one. Don't look at the vector of PoseStamped stamps. I think we're on the same page :-)

[tonynajjar]

then I don't see why we would not just keep it as is (compare current and incoming top-level stamps).

I just realized there is a problem with that. The BT input port is still a std::vector<geometry_msgs::msg::PoseStamped>, so no top-level stamp. We could change it to the new PoseStampedArray message but that would also require to change all other users of the std::vector<geometry_msgs::msg::PoseStamped> Goals (other BTs and NavigateThroughPoses navigator).
I would rather not do that in this PR. So what I propose is to keep it implemented as-such in this PR (check received timestamp is more recent than the most recent goal in vector) and make another PR to replace the vector with PoseStampedArray and change what timestamp we check.
What do you think?

[SteveMacenski]

I would rather not do that in this PR.

I agree. We should do that after the actual geometry_msgs option is merged so we don't fliplash people twice.

I'd just ask that you file a ticket for changing this to the new message and point to this node as also needing the update so its not forgotten. I'm OK with this as is on a temporary basis

[tonynajjar]

So to be clear, this PR is blocked both by the geometry_msgs option AND sequentially by a PR that switches all other users of std::vector<geometry_msgs::msg::PoseStamped> to geometry_msgs/PoseStampedArray.

[SteveMacenski]

OK - we could merge this now with nav2_msgs version and update geometry_msgs once that's merged - and then do the rest of the switches to use it. But also happy to wait if that's what you prefer

[tonynajjar]

Obviously waiting on https://github.com/ros2/common_interfaces/pull/262/files but you can check the rest meanwhile

[SteveMacenski]

Usual bits on docs and updating the groot xml index for the new input/output ports