Add asynchronous planning and execution

examples/ex_gripper.py

@@ -39,7 +39,6 @@ def main():
         closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS,
         gripper_group_name=panda.MOVE_GROUP_GRIPPER,
         callback_group=callback_group,
-        follow_joint_trajectory_action_name="gripper_trajectory_controller/follow_joint_trajectory",
         gripper_command_action_name="gripper_action_controller/gripper_cmd",
     )
 

examples/ex_joint_goal.py

@@ -2,6 +2,8 @@
 """
 Example of moving to a joint configuration.
 - 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]"
+- 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
+- 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:=0.0
 """
 
 from threading import Thread
@@ -10,7 +12,7 @@
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 
-from pymoveit2 import MoveIt2
+from pymoveit2 import MoveIt2, MoveIt2State
 from pymoveit2.robots import panda
 
 
@@ -33,6 +35,9 @@ def main():
             0.7853981633974483,
         ],
     )
+    node.declare_parameter("synchronous", True)
+    # If non-positive, don't cancel. Only used if synchronous is False
+    node.declare_parameter("cancel_after_secs", 0.0)
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -58,15 +63,49 @@ def main():
     moveit2.max_velocity = 0.5
     moveit2.max_acceleration = 0.5
 
-    # Get parameter
+    # Get parameters
     joint_positions = (
         node.get_parameter("joint_positions").get_parameter_value().double_array_value
     )
+    synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value
+    cancel_after_secs = (
+        node.get_parameter("cancel_after_secs").get_parameter_value().double_value
+    )
 
     # Move to joint configuration
     node.get_logger().info(f"Moving to {{joint_positions: {list(joint_positions)}}}")
     moveit2.move_to_configuration(joint_positions)
-    moveit2.wait_until_executed()
+    if synchronous:
+        # Note: the same functionality can be achieved by setting
+        # `synchronous:=false` and `cancel_after_secs` to a negative value.
+        moveit2.wait_until_executed()
+    else:
+        # Wait for the request to get accepted (i.e., for execution to start)
+        print("Current State: " + str(moveit2.query_state()))
+        rate = node.create_rate(10)
+        while moveit2.query_state() != MoveIt2State.EXECUTING:
+            rate.sleep()
+
+        # Get the future
+        print("Current State: " + str(moveit2.query_state()))
+        future = moveit2.get_execution_future()
+
+        # Cancel the goal
+        if cancel_after_secs > 0.0:
+            # Sleep for the specified time
+            sleep_time = node.create_rate(cancel_after_secs)
+            sleep_time.sleep()
+            # Cancel the goal
+            print("Cancelling goal")
+            moveit2.cancel_execution()
+
+        # Wait until the future is done
+        while not future.done():
+            rate.sleep()
+
+        # Print the result
+        print("Result status: " + str(future.result().status))
+        print("Result error code: " + str(future.result().result.error_code))
 
     rclpy.shutdown()
     executor_thread.join()

examples/ex_pose_goal.py

@@ -2,6 +2,8 @@
 """
 Example of moving to a pose goal.
 - 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
+- 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
+- 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:=0.0
 """
 
 from threading import Thread
@@ -10,7 +12,7 @@
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 
-from pymoveit2 import MoveIt2
+from pymoveit2 import MoveIt2, MoveIt2State
 from pymoveit2.robots import panda
 
 
@@ -24,6 +26,9 @@ def main():
     node.declare_parameter("position", [0.5, 0.0, 0.25])
     node.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0])
     node.declare_parameter("cartesian", False)
+    node.declare_parameter("synchronous", True)
+    # If non-positive, don't cancel. Only used if synchronous is False
+    node.declare_parameter("cancel_after_secs", 0.0)
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -53,13 +58,47 @@ def main():
     position = node.get_parameter("position").get_parameter_value().double_array_value
     quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
     cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value
+    synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value
+    cancel_after_secs = (
+        node.get_parameter("cancel_after_secs").get_parameter_value().double_value
+    )
 
     # Move to pose
     node.get_logger().info(
         f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
     )
     moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw, cartesian=cartesian)
-    moveit2.wait_until_executed()
+    if synchronous:
+        # Note: the same functionality can be achieved by setting
+        # `synchronous:=false` and `cancel_after_secs` to a negative value.
+        moveit2.wait_until_executed()
+    else:
+        # Wait for the request to get accepted (i.e., for execution to start)
+        print("Current State: " + str(moveit2.query_state()))
+        rate = node.create_rate(10)
+        while moveit2.query_state() != MoveIt2State.EXECUTING:
+            rate.sleep()
+
+        # Get the future
+        print("Current State: " + str(moveit2.query_state()))
+        future = moveit2.get_execution_future()
+
+        # Cancel the goal
+        if cancel_after_secs > 0.0:
+            # Sleep for the specified time
+            sleep_time = node.create_rate(cancel_after_secs)
+            sleep_time.sleep()
+            # Cancel the goal
+            print("Cancelling goal")
+            moveit2.cancel_execution()
+
+        # Wait until the future is done
+        while not future.done():
+            rate.sleep()
+
+        # Print the result
+        print("Result status: " + str(future.result().status))
+        print("Result error code: " + str(future.result().result.error_code))
 
     rclpy.shutdown()
     executor_thread.join()

pymoveit2/__init__.py

@@ -1,6 +1,6 @@
 from . import robots
 from .gripper_command import GripperCommand
 from .gripper_interface import GripperInterface
-from .moveit2 import MoveIt2
+from .moveit2 import MoveIt2, MoveIt2State
 from .moveit2_gripper import MoveIt2Gripper
 from .moveit2_servo import MoveIt2Servo

pymoveit2/gripper_interface.py

@@ -26,27 +26,38 @@ def __init__(
         skip_planning_fixed_motion_duration: float = 0.5,
         max_effort: float = 0.0,
         callback_group: Optional[CallbackGroup] = None,
-        follow_joint_trajectory_action_name: str = "gripper_trajectory_controller/follow_joint_trajectory",
+        follow_joint_trajectory_action_name: str = "DEPRECATED",
         gripper_command_action_name: str = "gripper_action_controller/gripper_cmd",
+        use_move_group_action: bool = False,
     ):
         """
         Combination of `MoveIt2Gripper` and `GripperCommand` interfaces that automatically
         selects the appropriate interface based on the available actions.
         """
 
+        # Check for deprecated parameters
+        if execute_via_moveit:
+            node.get_logger().warn(
+                "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead."
+            )
+            use_move_group_action = True
+        if follow_joint_trajectory_action_name != "DEPRECATED":
+            node.get_logger().warn(
+                "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead."
+            )
+
         MoveIt2Gripper.__init__(
             self=self,
             node=node,
             gripper_joint_names=gripper_joint_names,
             open_gripper_joint_positions=open_gripper_joint_positions,
             closed_gripper_joint_positions=closed_gripper_joint_positions,
             gripper_group_name=gripper_group_name,
-            execute_via_moveit=execute_via_moveit,
             ignore_new_calls_while_executing=ignore_new_calls_while_executing,
             skip_planning=skip_planning,
             skip_planning_fixed_motion_duration=skip_planning_fixed_motion_duration,
             callback_group=callback_group,
-            follow_joint_trajectory_action_name=follow_joint_trajectory_action_name,
+            use_move_group_action=use_move_group_action,
         )
 
         GripperCommand.__init__(
@@ -66,7 +77,7 @@ def __init__(
     def __determine_interface(self, timeout_sec=1.0):
         if self.gripper_command_action_client.wait_for_server(timeout_sec=timeout_sec):
             self._interface = GripperCommand
-        elif self.follow_joint_trajectory_action_client.wait_for_server(
+        elif self._execute_trajectory_action_client.wait_for_server(
             timeout_sec=timeout_sec
         ):
             self._interface = MoveIt2Gripper