-
Notifications
You must be signed in to change notification settings - Fork 984
Closed
Labels
Description
Is your feature request related to a problem? Please describe.
As mentioned in #3562, I created a threaded alternative to the MoveActionCapability. It works fine, but if I lock the PlanningSceneMonitor during planning1, there is an issue :
- The PlanningSceneMonitor is locked for a few seconds while the planning is done.
- The PlanningSceneMonitor inner CurrentStateMonitor receives JointStates, calling the
PlanningSceneMonitor::onStateUpdatecallback and eventually lock on the PlanningSceneMonitor mutex inPlanningSceneMonitor::updateSceneWithCurrentState. - Meanwhile, if I ask for a trajectory execution, the
TrajectoryExecutionManagerwill wait 1s forCurrentStateMonitor::waitForCurrentState. However, its CurrentStateMonitor is actually the PlanningSceneMonitor CurrentStateMonitor, which is potentially blocked and outdated. - The TrajectoryExecutionManager timeout and refuse the trajectory.
Describe the solution you'd like
I would like the TrajectoryExecutionManager to be able to run without blocking on the PlanningSceneMonitor.
Describe alternatives you've considered
We could have a second CurrentStateMonitor for the TrajectoryExecutionManager, separate from the PlanningSceneMonitor.
Additional context
1 I tried to clone the scene and unlock the PlanningSceneMonitor, but PlanningScene::clone doesn't clone the octomap.
Reactions are currently unavailable