Ports moveit #3676 and #3682 (backport #3283)#3317
Conversation
* 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 <rhaschke@techfak.uni-bielefeld.de> * 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 <me@v4hn.de> * Ports changes to ROS2. --------- Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de> Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com> Co-authored-by: Michael Görner <me@v4hn.de> Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai> (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
|
Cherry-pick of ba35aaa has failed: 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 |
|
@rr-mark could you take a look at the Humble conflicts for this one? Thanks! |
* Resolves merge conflicts. * Undoes erroneous auto-format. * Undoes erroneous auto-format.
|
Codecov ReportAttention: Patch coverage is
❗ Your organization needs to install the Codecov GitHub app to enable full functionality. Additional details and impacted files@@ Coverage Diff @@
## humble #3317 +/- ##
==========================================
- Coverage 51.39% 51.37% -0.02%
==========================================
Files 382 382
Lines 31896 31874 -22
==========================================
- Hits 16391 16372 -19
+ Misses 15505 15502 -3 ☔ View full report in Codecov by Sentry. |
Description
Checklist
This is an automatic backport of pull request #3283 done by [Mergify](https://mergify.com).