Ports moveit #3676 and #3682 #3283

rr-mark · 2025-01-30T13:24:40Z

Description

As Ports #3676 and #3682 from ros1 moveit #3247 but with history preserved properly.
Please back-port to Humble and Jazzy when merged

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

…676) This allows parallel execution + planning. Also required modifying updateSceneWithCurrentState() to allow skipping a scene update with a new robot state (from CurrentStateMonitor), if the planning scene is currently locked (due to planning). Otherwise, the CurrentStateMonitor would block too. Co-authored-by: Robert Haschke <[email protected]>

* Move update of state_update_pending_ to updateSceneWithCurrentState() * Revert to try_lock While there are a few other locks except explicit user locks (getPlanningSceneServiceCallback(), collisionObjectCallback(), attachObjectCallback(), newPlanningSceneCallback(), and scenePublishingThread()), these occur rather seldom (scenePublishingThread() publishes at 2Hz). Hence, we might indeed balance a non-blocking CSM vs. missed PS updates in favour of CSM. * Don't block for scene update from stateUpdateTimerCallback too The timer callback and CSM's state update callbacks are served from the same callback queue, which would block CSM again. * further locking adaptations reading dt_state_update_ and last_robot_state_update_wall_time_ does not lead to logic errors, but at most to a skipped or redundant update on corrupted data. Alternatively we could be on the safe side and turn both variables into std::atomic, but that would effectively mean locks on every read. Instead, only set state_update_pending_ as an atomic, which is lockfree in this case. Co-authored-by: Michael Görner <[email protected]>

codecov-commenter · 2025-01-30T13:48:17Z

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

Codecov Report

Attention: Patch coverage is 88.57143% with 4 lines in your changes missing coverage. Please review.

Project coverage is 45.58%. Comparing base (fbdd8c5) to head (98126c0).
Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...nning_scene_monitor/src/planning_scene_monitor.cpp	83.34%	4 Missing ⚠️

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

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3283      +/-   ##
==========================================
- Coverage   45.60%   45.58%   -0.01%     
==========================================
  Files         716      716              
  Lines       62388    62375      -13     
  Branches     7547     7545       -2     
==========================================
- Hits        28446    28428      -18     
- Misses      33776    33779       +3     
- Partials      166      168       +2

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

sea-bass

LGTM!

rr-mark · 2025-02-04T15:51:07Z

Looks like another sporadic test failure

sea-bass · 2025-02-04T15:58:05Z

Looks like another sporadic test failure

Yeah, it's pretty bad lately. I tried looking at this briefly this weekend but on my machine, even the most basic Pilz unit test always exits with an error due to bad memory freeing of shared pointers.

* Use separate callback queue + spinner for ExecuteTrajectoryAction (#3676) This allows parallel execution + planning. Also required modifying updateSceneWithCurrentState() to allow skipping a scene update with a new robot state (from CurrentStateMonitor), if the planning scene is currently locked (due to planning). Otherwise, the CurrentStateMonitor would block too. Co-authored-by: Robert Haschke <[email protected]> * PSM: simplify state_update_pending_ (#3682) * Move update of state_update_pending_ to updateSceneWithCurrentState() * Revert to try_lock While there are a few other locks except explicit user locks (getPlanningSceneServiceCallback(), collisionObjectCallback(), attachObjectCallback(), newPlanningSceneCallback(), and scenePublishingThread()), these occur rather seldom (scenePublishingThread() publishes at 2Hz). Hence, we might indeed balance a non-blocking CSM vs. missed PS updates in favour of CSM. * Don't block for scene update from stateUpdateTimerCallback too The timer callback and CSM's state update callbacks are served from the same callback queue, which would block CSM again. * further locking adaptations reading dt_state_update_ and last_robot_state_update_wall_time_ does not lead to logic errors, but at most to a skipped or redundant update on corrupted data. Alternatively we could be on the safe side and turn both variables into std::atomic, but that would effectively mean locks on every read. Instead, only set state_update_pending_ as an atomic, which is lockfree in this case. Co-authored-by: Michael Görner <[email protected]> * Ports changes to ROS2. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> Co-authored-by: Sebastian Castro <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> (cherry picked from commit ba35aaa) # Conflicts: # moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp # moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp # moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

* Use separate callback queue + spinner for ExecuteTrajectoryAction (#3676) This allows parallel execution + planning. Also required modifying updateSceneWithCurrentState() to allow skipping a scene update with a new robot state (from CurrentStateMonitor), if the planning scene is currently locked (due to planning). Otherwise, the CurrentStateMonitor would block too. Co-authored-by: Robert Haschke <[email protected]> * PSM: simplify state_update_pending_ (#3682) * Move update of state_update_pending_ to updateSceneWithCurrentState() * Revert to try_lock While there are a few other locks except explicit user locks (getPlanningSceneServiceCallback(), collisionObjectCallback(), attachObjectCallback(), newPlanningSceneCallback(), and scenePublishingThread()), these occur rather seldom (scenePublishingThread() publishes at 2Hz). Hence, we might indeed balance a non-blocking CSM vs. missed PS updates in favour of CSM. * Don't block for scene update from stateUpdateTimerCallback too The timer callback and CSM's state update callbacks are served from the same callback queue, which would block CSM again. * further locking adaptations reading dt_state_update_ and last_robot_state_update_wall_time_ does not lead to logic errors, but at most to a skipped or redundant update on corrupted data. Alternatively we could be on the safe side and turn both variables into std::atomic, but that would effectively mean locks on every read. Instead, only set state_update_pending_ as an atomic, which is lockfree in this case. Co-authored-by: Michael Görner <[email protected]> * Ports changes to ROS2. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> Co-authored-by: Sebastian Castro <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> (cherry picked from commit ba35aaa)

* Use separate callback queue + spinner for ExecuteTrajectoryAction (#3676) This allows parallel execution + planning. Also required modifying updateSceneWithCurrentState() to allow skipping a scene update with a new robot state (from CurrentStateMonitor), if the planning scene is currently locked (due to planning). Otherwise, the CurrentStateMonitor would block too. Co-authored-by: Robert Haschke <[email protected]> * PSM: simplify state_update_pending_ (#3682) * Move update of state_update_pending_ to updateSceneWithCurrentState() * Revert to try_lock While there are a few other locks except explicit user locks (getPlanningSceneServiceCallback(), collisionObjectCallback(), attachObjectCallback(), newPlanningSceneCallback(), and scenePublishingThread()), these occur rather seldom (scenePublishingThread() publishes at 2Hz). Hence, we might indeed balance a non-blocking CSM vs. missed PS updates in favour of CSM. * Don't block for scene update from stateUpdateTimerCallback too The timer callback and CSM's state update callbacks are served from the same callback queue, which would block CSM again. * further locking adaptations reading dt_state_update_ and last_robot_state_update_wall_time_ does not lead to logic errors, but at most to a skipped or redundant update on corrupted data. Alternatively we could be on the safe side and turn both variables into std::atomic, but that would effectively mean locks on every read. Instead, only set state_update_pending_ as an atomic, which is lockfree in this case. Co-authored-by: Michael Görner <[email protected]> * Ports changes to ROS2. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> (cherry picked from commit ba35aaa) Co-authored-by: Mark Johnson <[email protected]>

* Ports moveit #3676 and #3682 (#3283) * Use separate callback queue + spinner for ExecuteTrajectoryAction (#3676) This allows parallel execution + planning. Also required modifying updateSceneWithCurrentState() to allow skipping a scene update with a new robot state (from CurrentStateMonitor), if the planning scene is currently locked (due to planning). Otherwise, the CurrentStateMonitor would block too. Co-authored-by: Robert Haschke <[email protected]> * PSM: simplify state_update_pending_ (#3682) * Move update of state_update_pending_ to updateSceneWithCurrentState() * Revert to try_lock While there are a few other locks except explicit user locks (getPlanningSceneServiceCallback(), collisionObjectCallback(), attachObjectCallback(), newPlanningSceneCallback(), and scenePublishingThread()), these occur rather seldom (scenePublishingThread() publishes at 2Hz). Hence, we might indeed balance a non-blocking CSM vs. missed PS updates in favour of CSM. * Don't block for scene update from stateUpdateTimerCallback too The timer callback and CSM's state update callbacks are served from the same callback queue, which would block CSM again. * further locking adaptations reading dt_state_update_ and last_robot_state_update_wall_time_ does not lead to logic errors, but at most to a skipped or redundant update on corrupted data. Alternatively we could be on the safe side and turn both variables into std::atomic, but that would effectively mean locks on every read. Instead, only set state_update_pending_ as an atomic, which is lockfree in this case. Co-authored-by: Michael Görner <[email protected]> * Ports changes to ROS2. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> Co-authored-by: Sebastian Castro <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> (cherry picked from commit ba35aaa) # Conflicts: # moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp # moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp # moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp * Resolves merge conflicts. (#3322) * Resolves merge conflicts. * Undoes erroneous auto-format. * Undoes erroneous auto-format. --------- Co-authored-by: Mark Johnson <[email protected]>

rr-mark and others added 3 commits January 30, 2025 13:24

Ports changes to ROS2.

7a7fd55

rr-mark force-pushed the ports_moveit_#3676_#3682 branch from ea4d332 to 7a7fd55 Compare January 30, 2025 13:25

rr-mark mentioned this pull request Jan 30, 2025

Ports #3676 and #3682 from ros1 moveit #3247

Closed

6 tasks

sea-bass approved these changes Feb 4, 2025

View reviewed changes

Merge branch 'main' into ports_moveit_#3676_#3682

686f682

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 4, 2025

sjahr approved these changes Feb 5, 2025

View reviewed changes

Merge branch 'main' into ports_moveit_#3676_#3682

98126c0

sea-bass added this pull request to the merge queue Feb 6, 2025

Merged via the queue into moveit:main with commit ba35aaa Feb 6, 2025
8 of 9 checks passed

mergify bot mentioned this pull request Feb 6, 2025

Ports moveit #3676 and #3682 (backport #3283) #3317

Merged

6 tasks

mergify bot mentioned this pull request Feb 6, 2025

Ports moveit #3676 and #3682 (backport #3283) #3318

Merged

6 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Ports moveit #3676 and #3682 #3283

Ports moveit #3676 and #3682 #3283

rr-mark commented Jan 30, 2025

codecov-commenter commented Jan 30, 2025 •

edited

Loading

sea-bass left a comment

rr-mark commented Feb 4, 2025

sea-bass commented Feb 4, 2025 •

edited

Loading

Ports moveit #3676 and #3682 #3283

Ports moveit #3676 and #3682 #3283

Conversation

rr-mark commented Jan 30, 2025

Description

Checklist

codecov-commenter commented Jan 30, 2025 • edited Loading

Codecov Report

sea-bass left a comment

Choose a reason for hiding this comment

rr-mark commented Feb 4, 2025

sea-bass commented Feb 4, 2025 • edited Loading

codecov-commenter commented Jan 30, 2025 •

edited

Loading

sea-bass commented Feb 4, 2025 •

edited

Loading