New tem3

[cambel]

Event-based TEM (v3)

Summary

• Add a dynamic reconfigure variable allow_continuous_execution to enable non-blocking execution of trajectories.

• Events: When a new trajectory is pushed, it is immediately validated, by checking that the required controllers are active and not in use and that there are no collisions with active trajectories or the current state of the planning scene. Then, the trajectory is sent for execution in the corresponding controllers.
  The execution of each trajectory part will result in the event EXECUTION_COMPLETED being triggered. It marks the completion of the execution from the controller's side regardless of the status (SUCCEEDED, ABORTED, ...). If the status of the execution for a trajectory part is SUCCEEDED, we wait until all other parts are completed successfully. If the status of the trajectory is not successful, all other trajectory parts are canceled.
  The execution of a trajectory can result in the event EXECUTION_TIMEOUT being triggered. This occurs when the trajectory execution duration monitor is enabled and the trajectory takes longer to execute than expected. When triggered, all trajectory parts for this trajectory are canceled.

• Interdependent set of trajectories: The user can define a set of trajectories to be executed in strictly sequential order. In such cases, all the required controllers for the set of trajectories would be locked so that no other trajectory can use them. Example use case: In a picking task, the user would send a trajectory for the robot arm to get into a grasping pose and a trajectory for the gripper to close after reaching the grasping pose (two separate trajectories). After execution of this set of trajectories starts, new trajectories that attempt to use the gripper would be rejected (in the no backlog use case).

TODO

• Make the current blocking-mode execution use the event-based approach.
  • Implement plan_execution's is_path_valid in TEM
  • Re-implement getCurrentExpectedTrajectoryIndex()
  • Update wait_for_execution
• Add a path segment callback for the interdependent set of trajectories.
• Clean up debug messages
• Write unit tests
  • Simple simultaneous execution of trajectories
  • Waiting for a specific trajectory in simultaneous mode
  • Reject new trajectories
    • Controllers are being used
    • Collision with active trajectories
• Add support for backlog mode

[simonschmeisser]

Meta sounds to generic, maybe Ordered or Consecutive or Sequential?

[simonschmeisser]

You could add a temporary commit to use the modified moveit_msgs branch so ci works again

[simonschmeisser]

Instead of reverting I was thinking about adding a commit that modifies these lines
https://github.com/ros-planning/moveit/blob/master/.github/workflows/upstream.rosinstall#L4

[cambel]

The collision check of trajectories against the current state of the planning scene has been disabled for now. The test move_group_pick_place_test.test fails due to the gripper colliding with the object. Is the AllowCollisionMatrix not been updated to the "global" planning scene?

Some tests were also added.

TO DO:

• Fix trajectory's callback not being called sometimes

[robberthofmanfm]

This is beautiful. Thanks Cristian & co!

[simonschmeisser]

halfway through ...