Call ExecuteTrajectoryAction while planning by riv-mjohnson · Pull Request #3676 · moveit/moveit

riv-mjohnson · 2024-12-12T09:49:52Z

Description

Fixes Allow simultaneous planning and execution #3562
Fixes The TrajectoryExecutionManager uselessly block on the PlanningSceneMonitor #3563
Adds a dedicated spinner to MoveGroupExecuteTrajectoryAction, to allow it to run while other Capabilities are running.
Makes PlanningSceneMonitor::updateSceneWithCurrentState return early if the mutex is locked, rather than waiting for the mutex. This allows the CurrentStateMonitor to continue updating while the PlanningSceneMonitor is locked, allowing the TrajectoryExecutionManager to grab fresh states while planning is happening.

N.B. it was already possible to plan while a trajectory was executing with MoveGroupExecuteTrajectoryAction. This PR just allows new trajectories to start executing while planning.

Open questions:

Is this change to PlanningSceneMonitor the best way to avoid locking out the CurrentStateMonitor? Or do we want something a little more targeted to the specific callback the PlanningSceneMonitor adds to the CurrentStateMonitor?

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

codecov-commenter · 2024-12-12T11:31:36Z

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

Codecov Report

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

Project coverage is 0.00%. Comparing base (5e249e9) to head (0e6ce8e).
Report is 1 commits behind head on master.

Files with missing lines	Patch %	Lines
...nning_scene_monitor/src/planning_scene_monitor.cpp	0.00%	7 Missing ⚠️
...abilities/execute_trajectory_action_capability.cpp	0.00%	6 Missing ⚠️

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

Additional details and impacted files

@@            Coverage Diff             @@
##           master   #3676       +/-   ##
==========================================
- Coverage   47.85%   0.00%   -47.84%     
==========================================
  Files         604     582       -22     
  Lines       61108   57364     -3744     
  Branches     7029    7142      +113     
==========================================
- Hits        29235       0    -29235     
- Misses      31455   57364    +25909     
+ Partials      418       0      -418

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

rhaschke

Adding a second spinner not only allows for parallel processing of execution and planning requests, but of any two ROS events. I'm afraid, the code is not prepared for this concurrency.

riv-mjohnson · 2024-12-12T12:58:46Z

Sure, that's why I'm asking about mutexes. I'm thinking of something like a planning mutex and an executing mutex, so only planning+executing can happen concurrently, not e.g. planning+planning.

riv-mjohnson · 2024-12-12T13:00:27Z

This would be easier in ros2, with a separate spinner for the trajectory execution manager, but should still be achievable in ros1.

rhaschke · 2024-12-12T13:13:54Z

I was thinking of a separate spinner and callback queue for trajectory execution as well. This is possible in ROS1.
You just need to use a custom callback queue (instead of the default one) for all relevant subscribers, services, actions, etc.
https://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning#Advanced:_Using_Different_Callback_Queues

riv-mjohnson · 2024-12-12T13:26:59Z

Ooh, I did not know about this. I will take a look, thanks.

riv-mjohnson · 2024-12-12T15:34:33Z

@rhaschke I've replaced the second spinner thread with a dedicated spinner in MoveGroupExecuteTrajectoryAction.

~~I'm slightly surprised this works; I was expecting to need to add some kind of spinner to the CurrentStateMonitor, but my test setup seems happy.~~

^ Nevermind, I see the planning scene monitor has its own spinner, so the CurrentStateMonitor is happy. I'm happy that this solution should work.

riv-mjohnson · 2025-01-06T10:38:18Z

@rhaschke @sea-bass

Happy new year.

Could this PR get re-reviewed? I'm keen to get it merged in to ROS1 and then port to ROS2.

v4hn

I just looked into this for a while and the spinner for the separate queue should be fine.

The early return in updateSceneWithCurrentState seems problematic though and needs more details in my opinion.

Please rebase the branch to enable the required jammy and noble CI checks.

moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

v4hn

Personally I'm fine with the proposed patch, as it improves the current situation.
Thank you for contributing.
A clean rebase (instead of merging from master) would be appreciated.

Not sure whether you or @rhaschke want to address the bigger design issue (which needs some additional verification) of where to set state_update_pending_.

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

riv-mjohnson · 2025-01-07T10:08:37Z

A clean rebase (instead of merging from master) would be appreciated.

I think I've now rebased. I'm not very familiar with rebasing, so I don't know if it worked properly.

The network graph looks right, but the commit history is a mess, with most commits duplicated. I don't know if this is expected?

rhaschke · 2025-01-07T11:06:41Z

I think I've now rebased. I'm not very familiar with rebasing, so I don't know if it worked properly.

You managed to rebase locally. be49dc4 is the correct HEAD.

The network graph looks right, but the commit history is a mess, with most commits duplicated. I don't know if this is expected?

... but messed up subsequently by merging your rebased branch back into your PR branch.
You need to force-push be49dc4 to your PR branch to fix this:

git push --force-with-lease https://github.com/rivelinrobotics/rivelin_moveit be49dc416a16e8da9000ddd9d5cc7d5306c12210:execute_while_planning

riv-mjohnson · 2025-01-07T11:17:31Z

Thanks for the pointers.

I tried with be49dc4, but the linting failed, so I retried with c8b1952. This seems to duplicate the "Fixes formatting" commit, but does fix the formatting.

rhaschke

Generally, I approve this as well. However, I have some cleanup suggestions filed as rivelinrobotics#17.

riv-mjohnson · 2025-01-07T13:30:51Z

Does moveit use a style guide which covers the (bool skip_update_if_locked) vs (const bool skip_update_if_locked) case? You've both suggested it be changed in opposite directions.

rhaschke · 2025-01-07T13:37:20Z

Does moveit use a style guide which covers the (bool skip_update_if_locked) vs (const bool skip_update_if_locked) case?

As the argument is passed by value, const is not needed from the caller's perspective. Not using const allows the function to (re)use the variable in some other fashion. Declaring it const, forbids that too.
I think, MoveIt code tends to avoid const for passed-by-value args.

You've both suggested it be changed in opposite directions.

I'm not aware of that.

riv-mjohnson · 2025-01-07T13:38:28Z

Generally, I approve this as well. However, I have some cleanup suggestions filed as rivelinrobotics#17.

Thanks for these. I've left one comment on your PR: I'm not sure if the 0.1 second timeout will play nicely with the other timeouts when calling getCurrentState?

~~(I've also left a minor linting comment - I assume your auto formatter has the same wrong settings as mine).~~ - ignore this, it was caused by the reverting of too much, as below.

riv-mjohnson · 2025-01-07T13:47:21Z

You've both suggested it be changed in opposite directions.

I'm not aware of that.

Oh, sorry, ~~I think something is going wrong between your PR and the rebasing, which means your PR is undoing some of my PR? (Specifically the formatting error fixes and the const change)?~~

I think your commit to revert one of my commits reverted more than you intended? rivelinrobotics@94a71f3

riv-mjohnson · 2025-01-07T13:53:13Z

I've rebase-merged your PR into mine. I'll just fix the extra things that were reverted, one sec.

riv-mjohnson · 2025-01-07T13:59:32Z

Okay, I think everything is as it should be now.

riv-mjohnson · 2025-01-07T14:00:11Z

And yes, I think I misremembered the other timeouts being 0.1s rather than 1s, so the 0.1s timeout is fine. I am happy with all the changes here now.

…ectory execution manager.

…ectory_action_capability.

…' behaviour more specific.

…me if update succeeds.

- Rename queue_ -> callback_queue_ - Drop extra NodeHandle, but use existing root_node_handle_ of capability

…_wall_time if update succeeds." This reverts commit be49dc4.

v4hn · 2025-01-07T19:12:51Z

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

+      boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_, boost::defer_lock);
+      if (!skip_update_if_locked)
+        ulock.lock();
+      else if (!ulock.try_lock_for(boost::chrono::duration<double>(std::min(0.1, 0.1 * dt_state_update_.toSec()))))


at least formally dt_state_update_ is protected by state_pending_mutex_ which you don't hold here.
I don't think it's necessary to wait here at all.

Locking of the planning scene due to planning is only one locking scenario. Most of them hold the lock only shortly, e.g. by scenePublishingThread(), getPlanningSceneServiceCallback(), clearOctomap(), etc.
Thus, I thought that we should wait for the lock to become available and correctly finish the update.
Otherwise, we might get very irregular update frequencies most of the time.
However, I agree that we might want to use a fixed duration:

- else if (!ulock.try_lock_for(boost::chrono::duration<double>(std::min(0.1, 0.1 * dt_state_update_.toSec())))) + else if (!ulock.try_lock_for(boost::chrono::milliseconds(100)))

The concrete duration is debatable. Probably, 10ms are fine too. I have no idea. Ideally, we should collect some real-world statistics...
Anyway: In the past we waited forever. Thus 100ms is a tremendous improvement.

at least formally dt_state_update_ is protected by state_pending_mutex_ which you don't hold here.

I removed dt_state_update_ in 57563e2 of #3682.

…veit#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>

riv-mjohnson requested a review from rhaschke as a code owner December 12, 2024 09:49

rhaschke reviewed Dec 12, 2024

View reviewed changes

riv-mjohnson requested a review from rhaschke December 13, 2024 12:36

riv-mjohnson changed the title ~~Execute while planning~~ Call ExecuteTrajectoryAction while planning Jan 6, 2025

v4hn reviewed Jan 6, 2025

View reviewed changes

v4hn approved these changes Jan 6, 2025

View reviewed changes

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp Outdated Show resolved Hide resolved

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp Show resolved Hide resolved

riv-mjohnson force-pushed the execute_while_planning branch from dc1a8b9 to be49dc4 Compare January 7, 2025 11:12

rhaschke reviewed Jan 7, 2025

View reviewed changes

rhaschke approved these changes Jan 7, 2025

View reviewed changes

riv-mjohnson added 4 commits January 7, 2025 15:09

Adds second thread to move_group spinner. Fixes state timeout in traj…

fb5486a

…ectory execution manager.

Replaces second spinner thread with dedicated spinner in execute_traj…

306a102

…ectory_action_capability.

Adds to execute action destructor; makes behaviour more specific.

bd27602

Adds 'spinner_.stop' to execute action destructor; makes 'try_to_lock…

90fd629

…' behaviour more specific.

riv-mjohnson and others added 10 commits January 7, 2025 15:09

Fixes formatting.

2619351

Fixes typo.

744b233

Only updates state_update_pending and last_robot_state_update_wall_ti…

c16da93

…me if update succeeds.

Fixes formatting.

2bb9d46

Cleanup

66ad9df

- Rename queue_ -> callback_queue_ - Drop extra NodeHandle, but use existing root_node_handle_ of capability

Revert "Only updates state_update_pending and last_robot_state_update…

db9d5bd

…_wall_time if update succeeds." This reverts commit be49dc4.

Simplify locking code

fc21a85

try_lock_for 0.1s

90a49a1

Fixes formatting.

09835cd

Removes const from bool argument.

0e6ce8e

riv-mjohnson force-pushed the execute_while_planning branch from d19ecf8 to 0e6ce8e Compare January 7, 2025 15:09

rhaschke merged commit 3aed1dc into moveit:master Jan 7, 2025

rhaschke mentioned this pull request Jan 7, 2025

PSM: Fix handling of flag state_update_pending_ #3682

Merged

v4hn reviewed Jan 7, 2025

View reviewed changes

riv-mjohnson mentioned this pull request Jan 16, 2025

Ports #3676 and #3682 from ros1 moveit moveit/moveit2#3247

Closed

6 tasks

This was referenced Jan 22, 2025

Updating rviz planning scene when monitored planning scene is locked #3688

Closed

regression due to updated locking of PSM's scene updates #3691

Closed

Conversation

riv-mjohnson commented Dec 12, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Checklist

Uh oh!

codecov-commenter commented Dec 12, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

rhaschke left a comment

Choose a reason for hiding this comment

Uh oh!

riv-mjohnson commented Dec 12, 2024

Uh oh!

riv-mjohnson commented Dec 12, 2024

Uh oh!

rhaschke commented Dec 12, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

riv-mjohnson commented Dec 12, 2024

Uh oh!

riv-mjohnson commented Dec 12, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

riv-mjohnson commented Jan 6, 2025

Uh oh!

v4hn left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

v4hn left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

riv-mjohnson commented Jan 7, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

rhaschke commented Jan 7, 2025

Uh oh!

riv-mjohnson commented Jan 7, 2025

Uh oh!

rhaschke left a comment

Choose a reason for hiding this comment

Uh oh!

riv-mjohnson commented Jan 7, 2025

Uh oh!

rhaschke commented Jan 7, 2025

Uh oh!

riv-mjohnson commented Jan 7, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

riv-mjohnson commented Jan 7, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

riv-mjohnson commented Jan 7, 2025

Uh oh!

riv-mjohnson commented Jan 7, 2025

Uh oh!

riv-mjohnson commented Jan 7, 2025

Uh oh!

v4hn Jan 7, 2025

Choose a reason for hiding this comment

Uh oh!

rhaschke Jan 7, 2025

Choose a reason for hiding this comment

Uh oh!

rhaschke Jan 7, 2025

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

riv-mjohnson commented Dec 12, 2024 •

edited

Loading

codecov-commenter commented Dec 12, 2024 •

edited

Loading

rhaschke commented Dec 12, 2024 •

edited

Loading

riv-mjohnson commented Dec 12, 2024 •

edited

Loading

riv-mjohnson commented Jan 7, 2025 •

edited

Loading

riv-mjohnson commented Jan 7, 2025 •

edited

Loading

riv-mjohnson commented Jan 7, 2025 •

edited

Loading