fix: ensure attached objects update during motion execution

[MarcoMagriDev]

Description

• Check that attached objects in the monitored robot match those in the planned trajectory.
• If an object disappears from the monitored robot, remove it from the trajectory way-point robot_state. (avoid false positives, stop trajectories due to collisions between environment and objects that are not attached to the robot anymore)
• If an object is attached to the monitored robot but missing in the trajectory, add it to enable meaningful collision checking. (avoids disruptive condition in which even if the object has been added to the robot as attached collision object it is not considered for online collision checking)

---

Implications

This could impact a few different situations:

• Imagine you’re catching an object during motion, and an external node tells the planning scene to add the new attached collision object. Even though MoveIt knows about the new attached object, the collision check still only considers the attached objects that were there at the planning stage, so it misses the new one. This could lead to a situation where a collision is missed because the newly attached object isn’t part of the check.
• Similarly, dropping an object during motion may result in preempting a valid path due to a non existsent collision.

[sea-bass]

Thank you for this!

[MarcoMagriDev]

The only simplification I could think of is that, under the assumption that "attached objects are set to the waypoint's robot state at planning time" (which actually holds) we could query sample_attached_object only once. However, I proposed updating it for each waypoint to ensure robustness, even if this assumption doesn't hold. Do you see any potential issues or improvements with this?

[sea-bass]

Ah thanks for asking.

I guess I would be thinking about:

1. Does it slow things down to be potentially attaching/detaching objects at each waypoint during this check?
2. It's difficult for me to know what the user's intent would be on a case by case basis. Right now it seems like the current state is treated as the source of truth, but I'm unsure whether users want that in every case. I guess the previous implementation did the opposite and treated the attached objects from the pre-planned waypoint as the source of truth.

Is it worth maybe adding a flag to this function for whether one wants to prioritize current state vs. planned states? And elevate this up to the config/parameter level?

[MarcoMagriDev]

1. Does it slow things down to be potentially attaching/detaching objects at each waypoint during this check?

I think it depends on how frequently the planning scene changes. In a highly dynamic environment, this method would be called often, which could impact performance. Since attached objects are processed for all robot states associated with waypoints after the currently executed one, the worst-case scenario with the current implementation occurs when scene updates happen frequently at the start of the trajectory.

With the proposed simplification, this reduces to simply "high-frequency scene updates," as only one robot state from the trajectory is queried for attached objects, while the rest are assumed unchanged.

On the other hand, if future plans include attached objects that are not consistently present throughout the motion, this simplification could introduce errors.

2. It's difficult for me to know what the user's intent would be on a case by case basis. Right now it seems like the current state is treated as the source of truth, but I'm unsure whether users want that in every case. I guess the previous implementation did the opposite and treated the attached objects from the pre-planned waypoint as the source of truth.

Is it worth maybe adding a flag to this function for whether one wants to prioritize current state vs. planned states? And elevate this up to the config/parameter level?

As for prioritizing the current state vs. planned states, since world objects are always considered in real-time rather than at planning time, I don’t see a strong reason to let the user choose the source of truth for attached collision objects. Keeping it consistent with real-time world objects could help prevent many erroneous situations.

[sea-bass]

Agreed. But at least in your comments on 1., maybe we do need the toggle on using the attached objects only at the start vs. updating this frequently.

Basically, asking the user "do you want this simplification or not?"

[MarcoMagriDev]

I just pushed a new commit that automates this process without requiring user input.

Before entering the monitoring phase, it verifies whether the attached objects remain consistent throughout the trajectory. If they do, they are stored and later used by the isRemainingPathValid method without needing to be queried again.

If the attached objects change during the planned trajectory, the map is left empty, signaling isRemainingPathValid to query them at each waypoint.

I believe this approach is more robust than using a parameter, as it eliminates the possibility of misconfiguration by the user. What do you think?

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 0% with 38 lines in your changes missing coverage. Please review.

Project coverage is 45.56%. Comparing base (9922704) to head (8ccb626).

Files with missing lines	Patch %	Lines
...ros/planning/plan_execution/src/plan_execution.cpp	0.00%	33 Missing ⚠️
moveit_core/robot_state/src/robot_state.cpp	0.00%	5 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3327      +/-   ##
==========================================
- Coverage   45.60%   45.56%   -0.04%     
==========================================
  Files         716      716              
  Lines       62443    62475      +32     
  Branches     7558     7566       +8     
==========================================
- Hits        28471    28458      -13     
- Misses      33805    33850      +45     
  Partials      167      167              

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.