Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix: ensure attached objects update during motion execution #3327

Open
wants to merge 8 commits into
base: main
Choose a base branch
from

Conversation

MarcoMagriDev
Copy link
Contributor

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.

- 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 waypoint robot_state.
- If an object is attached to the monitored robot but missing in the trajectory, add it to enable meaningful collision checking.
@sea-bass sea-bass added backport-humble Mergify label that triggers a PR backport to Humble backport-jazzy Mergify label that triggers a PR backport to Jazzy labels Feb 6, 2025
@codecov-commenter
Copy link

codecov-commenter commented Feb 6, 2025

⚠️ Please install the 'codecov app svg image' to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

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

Project coverage is 45.57%. Comparing base (7db0bd4) to head (db6d93f).

Files with missing lines Patch % Lines
...ros/planning/plan_execution/src/plan_execution.cpp 0.00% 37 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.97%   45.57%   -0.39%     
==========================================
  Files         716      716              
  Lines       62477    62491      +14     
  Branches     7564     7572       +8     
==========================================
- Hits        28715    28475     -240     
- Misses      33594    33850     +256     
+ Partials      168      166       -2     

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

MarcoMagriDev and others added 3 commits February 7, 2025 16:06
style: `sample_attached_object` -> `sample_attached_objects`
…queried based on their consistency in the planned trajectory.

---
Before entering the monitoring phase, it checks 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.
Copy link
Contributor

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Mostly looks good to me, with some minor comments!

Also tagged a few other maintainers for a second look.

moveit_ros/planning/plan_execution/src/plan_execution.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/plan_execution/src/plan_execution.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/plan_execution/src/plan_execution.cpp Outdated Show resolved Hide resolved
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
backport-humble Mergify label that triggers a PR backport to Humble backport-jazzy Mergify label that triggers a PR backport to Jazzy
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants