Skip to content

The TrajectoryExecutionManager uselessly block on the PlanningSceneMonitor #3563

@Hugal31

Description

@Hugal31

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 :

  1. The PlanningSceneMonitor is locked for a few seconds while the planning is done.
  2. The PlanningSceneMonitor inner CurrentStateMonitor receives JointStates, calling the PlanningSceneMonitor::onStateUpdate callback and eventually lock on the PlanningSceneMonitor mutex in PlanningSceneMonitor::updateSceneWithCurrentState.
  3. Meanwhile, if I ask for a trajectory execution, the TrajectoryExecutionManager will wait 1s for CurrentStateMonitor::waitForCurrentState. However, its CurrentStateMonitor is actually the PlanningSceneMonitor CurrentStateMonitor, which is potentially blocked and outdated.
  4. 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.

Metadata

Metadata

Assignees

No one assigned

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions