Add asynchronous planning and execution

[amalnanavati]

Description

The primary goal of this PR is to enable users of pymoveit2 to plan and/or execute asynchronously. This is useful to allow users to cancel planning/execution calls, and to allow the main thread to continue executing during planning/execution (e.g., we use pymoveit2 within a behavior tree framework, so the tree must be able to continue ticking as the robot is moving).

Changes 1-2 below tackle that primary goal. As part of tackling the primary goal, some additional changes also made sense; those are Changes 3-4, which are justified below.

Detailed Changes

1. Make action execution (both MoveGroup and controller execution) asynchronous. Track the state of the action with a new enum, MoveIt2State, and allow users to query the state and cancel execution.
2. Carve out the asynchronous planning logic into another function, plan_async, that users of this API can call directly. Create an affiliated get_trajectory function that processes the future and returns a trajectory.
3. Make all invocations of plan/plan_async use the planning service (earlier users could decide whether to use the service or the MoveGroup action with plan_only=true).
   1. The reason for this is that the way of interacting with asynchronous actions call is very different from asynch services, since action calls first return a goal handle, whereas service calls directly return a future for the result. This would make the return value of plan_async, and how users interact with that return value, quite different depending on whether they invoke the action or the service.
   2. Since this code already has a way to asynchronously call, query, and cancel a MoveGroup action (via Change 1 above), I think a better way to re-enable planning-only via MoveGroup would be to allow users to set the plan_only property of the MoveGroup goal, and then allow them to invoke _send_goal_async_move_action (which is invoked anyway if they use move_to_pose or move_to_configuration). This is unimplemented — let me know if you’d like it.
4. Replace the FollowJointTrajectory action exposed by the controller with the ExecuteTrajectory action exposed by MoveIt2.
   1. This is necessary to allow us to cancel the goal via MoveIt2’s /trajectory_execution_event topic. Although we could alternatively cancel the action through the goal handle, the action server is able to reject the cancellation request. This topic provides us a way to more directly stop execution, but it can only be done if we execute using MoveIt’s ExecuteTrajectory action.
   2. I don’t think any features are lost with this switch, since MoveIt’s ExecuteTrajectory internally calls the controller’s FollowJointTrajectory action.

Testing

• Launch the simulated panda arm: ros2 launch panda_moveit_config ex_fake_control.launch.py
• Test move_to_configuration:
  • Test the synchronous execution still works: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]"
  • Restore to initial joint positions: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[0.0, -0.7853981633974483, 0.0, -2.356194490192345, 0.0, 1.5707963267948966, 0.7853981633974483]"
  • Test the asynchronous execution can be successfully canceled: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False -p cancel_after_secs:=1.0
  • Restore to initial joint positions
  • Test that asynchronous execution succeeds if not canceled: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False -p cancel_after_secs:=-1.0
• Test move_to_pose:
  • Restore to initial joint positions
  • Test the synchronous execution still works: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
  • Restore to initial joint positions
  • Test the asynchronous execution can be successfully canceled: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=1.0
  • Restore to initial joint positions
  • Test that asynchronous execution succeeds if not canceled: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=-1.0
• Test gripper:
  • ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="open"
  • ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="close"
  • ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="toggle"

Further, for our application we have developed behavior tree behaviors that use this PR’s changes to asynchronously plan and execute (with use_move_group_action=False). We’ve been using those behaviors for months and they have worked reliably.

[amalnanavati]

(Note: I think squashing and merging would make the most sense given the many commits)

[AndrejOrsula]

Thank you very much for your contribution. It looks great!

I have two comments. One of them discusses the breaking changes of this PR. If it would be possible to hide/remove these breaking changes, it would be much simpler to merge this PR into the primary branch. Regardless, it is probably still better to merge it into devel as a beginning to give some time for testing.

[AndrejOrsula]

Also, I added CI action for pre-commit that might run automatically on the next commit. If not, could you please run your PR through pre-commit? There is a setup script that should simplify installing pre-commit and automatically running it for this repo. Thank you.

[amalnanavati]

Thanks for the prompt reply! I addressed all the comments, rebased onto and ran the pre-commit hook, and re-tested it. I think there is slight further discussion to be had on breaking changes (see comment).

Just to give you a heads-up, there will be more PRs coming over the coming days/weeks that build off of this. My labmates and I have been making multiple feature additions in our fork over the last few months, including expanding the API to allow for path constraints, modify the allowed collision matrix, move collision objects, scale collision meshes, etc. I'll create PRs for them one-by-one, and target devel like this PR does.

[AndrejOrsula]

Thank you for addressing those changes!

---

I just realized that this PR breaks MoveIt2Gripper and in turn GripperInterface because MoveIt2Gripper inherits from MoveIt2 and reuses some of its internals (including the replaced FollowJointTrajectory action client).

E.g. running ex_gripper.py:

'GripperInterface' object has no attribute '_MoveIt2__follow_joint_trajectory_action_client'

Would you be interested in updating this interface accordingly? Alternatively, we can merge this into devel now and then address grippers in some upcoming PR.

[amalnanavati]

Fixed the gripper interface, and updated the tests accordingly. Note that because I don't actively use the gripper I probably didn't test it as thoroughly as can be -- I just ran the three commands at the top ex_gripper.py. Feel free to test more if there are further edge cases you often encounter when using the Gripper Interface :)

[AndrejOrsula]

It works well from my initial tests. Thank you very much for your contribution! 😄