fix: ensure attached objects update during motion execution (backport #3327)

[mergify[bot]]

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.

  ---

  This is an automatic backport of pull request #3327 done by Mergify.

[mergify[bot]]

Cherry-pick of 89acaddda936efd4acdade37578bee6ce037f66b has failed:

On branch mergify/bp/humble/pr-3327
Your branch is up to date with 'origin/humble'.

You are currently cherry-picking commit 89acaddda.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
	modified:   moveit_core/robot_state/src/robot_state.cpp

Unmerged paths:
  (use "git add/rm <file>..." as appropriate to mark resolution)
	deleted by us:   moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp
	deleted by us:   moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp
	both modified:   moveit_ros/planning/plan_execution/src/plan_execution.cpp

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

[sea-bass]

@MarcoMagriDev would you like to attempt resolving these conflicts for the humble backport? Or also it could just not be backported as it is kind of a new feature.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[codecov[bot]]

Codecov Report

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

Project coverage is 0.00%. Comparing base (c1aa435) to head (de3b775). Report is 3 commits behind head on humble.

Additional details and impacted files

@@            Coverage Diff             @@
##           humble   #3413       +/-   ##
==========================================
- Coverage   50.94%       0   -50.93%     
==========================================
  Files         390       0      -390     
  Lines       32512       0    -32512     
==========================================
- Hits        16559       0    -16559     
+ Misses      15953       0    -15953     

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

:rocket: New features to boost your workflow:

• :snowflake: Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.