From 64283c4732d1175f48114c38bc397705caa6f97a Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Tue, 20 Jun 2023 15:01:10 -0700 Subject: [PATCH 01/18] Add joint goal example for Kinova JACO2 --- examples/ex_joint_goal.py | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index 45e97b2..0f6fa19 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -11,7 +11,7 @@ from rclpy.node import Node from pymoveit2 import MoveIt2 -from pymoveit2.robots import panda +from pymoveit2.robots import kinova def main(): @@ -24,13 +24,12 @@ def main(): node.declare_parameter( "joint_positions", [ - 0.0, - 0.0, - 0.0, - -0.7853981633974483, - 0.0, - 1.5707963267948966, - 0.7853981633974483, + -1.47568, + 2.92779, + 1.00845, + -2.0847, + 1.43588, + 1.32575 ], ) @@ -40,10 +39,10 @@ def main(): # Create MoveIt 2 interface moveit2 = MoveIt2( node=node, - joint_names=panda.joint_names(), - base_link_name=panda.base_link_name(), - end_effector_name=panda.end_effector_name(), - group_name=panda.MOVE_GROUP_ARM, + joint_names=kinova.joint_names(), + base_link_name=kinova.base_link_name(), + end_effector_name="forkTip", + group_name=kinova.MOVE_GROUP_ARM, callback_group=callback_group, ) From 5158e68710db5a34e93b9d3c5998ddef57a77055 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Tue, 20 Jun 2023 15:26:16 -0700 Subject: [PATCH 02/18] [WIP] Added execution cancellation and polling --- pymoveit2/moveit2.py | 364 +++++++++++++++++++++++++------------------ 1 file changed, 214 insertions(+), 150 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index eac7bc0..7fbd268 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -2,7 +2,6 @@ from typing import List, Optional, Tuple, Union from action_msgs.msg import GoalStatus -from control_msgs.action import FollowJointTrajectory from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion from moveit_msgs.action import MoveGroup from moveit_msgs.msg import ( @@ -20,6 +19,8 @@ GetPositionFK, GetPositionIK, ) + +import rclpy from rclpy.action import ActionClient from rclpy.callback_groups import CallbackGroup from rclpy.node import Node @@ -29,6 +30,7 @@ QoSProfile, QoSReliabilityPolicy, ) +from rclpy.task import Future from sensor_msgs.msg import JointState from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive from std_msgs.msg import Header @@ -49,7 +51,6 @@ def __init__( end_effector_name: str, group_name: str = "arm", execute_via_moveit: bool = False, - ignore_new_calls_while_executing: bool = False, callback_group: Optional[CallbackGroup] = None, follow_joint_trajectory_action_name: str = "joint_trajectory_controller/follow_joint_trajectory", ): @@ -61,10 +62,8 @@ def __init__( - `end_effector_name` - Name of the robot end effector - `group_name` - Name of the planning group for robot arm - `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) - FollowJointTrajectory action (controller) is employed otherwise + ExecuteTrajectory action is employed otherwise together with a separate planning service client - - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories - while previous is still being executed - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - `follow_joint_trajectory_action_name` - Name of the action server for the controller """ @@ -86,57 +85,58 @@ def __init__( callback_group=self._callback_group, ) - # Create action client for move action - self.__move_action_client = ActionClient( - node=self._node, - action_type=MoveGroup, - action_name="move_action", - goal_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - result_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - cancel_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - feedback_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - status_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) - - # Otherwise create a separate service client for planning - self._plan_kinematic_path_service = self._node.create_client( - srv_type=GetMotionPlan, - srv_name="plan_kinematic_path", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=callback_group, - ) - self.__kinematic_path_request = GetMotionPlan.Request() + if execute_via_moveit: + # Create action client for move action + self.__move_action_client = ActionClient( + node=self._node, + action_type=MoveGroup, + action_name="move_action", + goal_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + result_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + cancel_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + feedback_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + status_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) + else: + # Otherwise create a separate service client for planning + self._plan_kinematic_path_service = self._node.create_client( + srv_type=GetMotionPlan, + srv_name="plan_kinematic_path", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__kinematic_path_request = GetMotionPlan.Request() # Create a separate service client for Cartesian planning self._plan_cartesian_path_service = self._node.create_client( @@ -206,13 +206,11 @@ def __init__( end_effector=end_effector_name, ) - # Flag to determine whether to execute trajectories via MoveIt2, or rather by calling a separate action with the controller itself + # Flag to determine whether to execute trajectories via Move Group Action, or rather by calling + # the separate ExecuteTrajectory action # Applies to `move_to_pose()` and `move_to_configuraion()` self.__execute_via_moveit = execute_via_moveit - # Flag that determines whether a new goal can be send while the previous one is being executed - self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing - # Store additional variables for later use self.__joint_names = joint_names self.__base_link_name = base_link_name @@ -223,11 +221,76 @@ def __init__( self.__is_motion_requested = False self.__is_executing = False self.motion_suceeded = False + self.__cancellation_future = None + self.__execution_goal_handle = None + self.__last_error_code = None self.__wait_until_executed_rate = self._node.create_rate(1000.0) + self.__execution_mutex = threading.Lock() # Event that enables waiting until async future is done self.__future_done_event = threading.Event() + #### Execution Polling Functions + from enum import Enum + class MoveIt2State(Enum): + IDLE = 0 + REQUESTING = 1 + EXECUTING = 2 + CANCELING = 3 + + def query_state(self) -> MoveIt2State: + with self.__execution_mutex: + if self.__is_motion_requested: + return MoveIt2State.REQUESTING + elif self.__is_executing: + if self.__cancellation_future is None: + return MoveIt2State.EXECUTING + else: + return MoveIt2State.CANCELING + else return MoveIt2State.IDLE + + def cancel_execution(self) -> Optional[Future]: + with self.__execution_mutex: + if self.query_state() != MoveIt2State.EXECUTING: + self._node.get_logger().warn( + "Attempted to cancel without active goal." + ) + return None + + self.__cancellation_future = self.__execution_goal_handle.cancel_goal_async() + self.__cancellation_future.add_done_callback( + self.__cancel_callback + ) + + return self.__cancellation_future + + def __cancel_callback(self, response): + self.__execution_mutex.acquire() + cancel_response = response.result() + if len(cancel_response.goals_canceling) > 0: + self.__node.get_logger().info('Execution successfully canceled') + self.__is_executing = False + self.__execution_goal_handle = None + self.__cancellation_future = None + else: + self.__node.get_logger().error('Execution failed to cancel') + self.__execution_mutex.release() + + def get_execution_future(self) -> Optional[Future]: + with self.__execution_mutex: + if self.query_state() != MoveIt2State.EXECUTING: + self._node.get_logger().warn( + "Need active goal for future." + ) + return None + + return self.__execution_goal_handle.get_result_async() + + def get_last_execution_error_code(self) -> Optional[MoveItErrorCodes]: + return self.__last_error_code + + #### + def move_to_pose( self, pose: Optional[Union[PoseStamped, Pose]] = None, @@ -283,12 +346,11 @@ def move_to_pose( ) if self.__execute_via_moveit and not cartesian: - if self.__ignore_new_calls_while_executing and self.__is_executing: + if self.__is_motion_requested or self.__is_executing: self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) return - self.__is_motion_requested = True # Set goal self.set_pose_goal( @@ -340,12 +402,11 @@ def move_to_configuration( """ if self.__execute_via_moveit: - if self.__ignore_new_calls_while_executing and self.__is_executing: + if self.__is_motion_requested or self.__is_executing: self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) return - self.__is_motion_requested = True # Set goal self.set_joint_goal( @@ -395,6 +456,55 @@ def plan( start_joint_state: Optional[Union[JointState, List[float]]] = None, cartesian: bool = False, ) -> Optional[JointTrajectory]: + """ + Call plan_async and wait on future + """ + future = self.plan(**locals()) + + if future is None: + return None + + rclpy.spin_until_future_complete(self._node, future) + res = future.result() + + # Cartesian + if cartesian: + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.solution.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + + # Else Kinematic + res = res.motion_plan_response + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.trajectory.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + + def plan_async( + self, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + joint_positions: Optional[List[float]] = None, + joint_names: Optional[List[str]] = None, + frame_id: Optional[str] = None, + tolerance_position: float = 0.001, + tolerance_orientation: float = 0.001, + tolerance_joint_position: float = 0.001, + weight_position: float = 1.0, + weight_orientation: float = 1.0, + weight_joint_position: float = 1.0, + start_joint_state: Optional[Union[JointState, List[float]]] = None, + cartesian: bool = False, + ) -> Optional[Future]: """ Plan motion based on previously set goals. Optional arguments can be passed in to internally use `set_position_goal()`, `set_orientation_goal()` or `set_joint_goal()` @@ -488,37 +598,32 @@ def plan( elif self.joint_state is not None: self.__move_action_goal.request.start_state.joint_state = self.joint_state - # Plan trajectory by sending a goal (blocking) + # Plan trajectory asynchronously by service call if cartesian: - joint_trajectory = self._plan_cartesian_path( + future = self._plan_cartesian_path( frame_id=pose_stamped.header.frame_id if pose_stamped is not None else frame_id ) else: - if self.__execute_via_moveit: - # Use action client - joint_trajectory = self._send_goal_move_action_plan_only() - else: - # Use service - joint_trajectory = self._plan_kinematic_path() + # Use service + future = self._plan_kinematic_path() # Clear all previous goal constrains self.clear_goal_constraints() - return joint_trajectory + return future def execute(self, joint_trajectory: JointTrajectory): """ Execute joint_trajectory by communicating directly with the controller. """ - if self.__ignore_new_calls_while_executing and self.__is_executing: + if self.__is_motion_requested or self.__is_executing: self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) return - self.__is_motion_requested = True follow_joint_trajectory_goal = init_follow_joint_trajectory_goal( joint_trajectory=joint_trajectory @@ -528,7 +633,6 @@ def execute(self, joint_trajectory: JointTrajectory): self._node.get_logger().warn( "Cannot execute motion because the provided/planned trajectory is invalid." ) - self.__is_motion_requested = False return self._send_goal_async_follow_joint_trajectory(goal=follow_joint_trajectory_goal) @@ -957,8 +1061,10 @@ def force_reset_executing_state(self): used. This function is applicable only in a very few edge-cases, so it should almost never be used. """ + self.__execution_mutex.acquire() self.__is_motion_requested = False self.__is_executing = False + self.__execution_mutex.release() def add_collision_primitive( self, @@ -1308,40 +1414,7 @@ def __joint_state_callback(self, msg: JointState): self.__new_joint_state_available = True self.__joint_state_mutex.release() - def _send_goal_move_action_plan_only( - self, wait_for_server_timeout_sec: Optional[float] = 1.0 - ) -> Optional[JointTrajectory]: - # Set action goal to only do planning without execution - original_plan_only = self.__move_action_goal.planning_options.plan_only - self.__move_action_goal.planning_options.plan_only = True - - stamp = self._node.get_clock().now().to_msg() - self.__move_action_goal.request.workspace_parameters.header.stamp = stamp - - if not self.__move_action_client.wait_for_server( - timeout_sec=wait_for_server_timeout_sec - ): - self._node.get_logger().warn( - f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" - ) - return None - - move_action_result = self.__move_action_client.send_goal( - goal=self.__move_action_goal, - feedback_callback=None, - ) - - # Revert back to original planning/execution mode - self.__move_action_goal.planning_options.plan_only = original_plan_only - - if move_action_result.status == GoalStatus.STATUS_SUCCEEDED: - return move_action_result.result.planned_trajectory.joint_trajectory - else: - return None - - def _plan_kinematic_path( - self, wait_for_server_timeout_sec: Optional[float] = 1.0 - ) -> Optional[JointTrajectory]: + def _plan_kinematic_path(self) -> Optional[Future]: # Re-use request from move action goal self.__kinematic_path_request.motion_plan_request = ( self.__move_action_goal.request @@ -1359,32 +1432,21 @@ def _plan_kinematic_path( for orientation_constraint in constraints.orientation_constraints: orientation_constraint.header.stamp = stamp - if not self._plan_kinematic_path_service.wait_for_service( - timeout_sec=wait_for_server_timeout_sec - ): + if not self._plan_kinematic_path_service.service_is_ready(): self._node.get_logger().warn( f"Service '{self._plan_kinematic_path_service.srv_name}' is not yet available. Better luck next time!" ) return None - res = self._plan_kinematic_path_service.call( + return self._plan_kinematic_path_service.call_async( self.__kinematic_path_request - ).motion_plan_response - - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.trajectory.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None + ) def _plan_cartesian_path( self, max_step: float = 0.0025, - wait_for_server_timeout_sec: Optional[float] = 1.0, frame_id: Optional[str] = None, - ) -> Optional[JointTrajectory]: + ) -> Optional[Future]: # Re-use request from move action goal self.__cartesian_path_request.start_state = ( self.__move_action_goal.request.start_state @@ -1430,39 +1492,26 @@ def _plan_cartesian_path( self.__cartesian_path_request.waypoints = [target_pose] - if not self._plan_cartesian_path_service.wait_for_service( - timeout_sec=wait_for_server_timeout_sec - ): + if not self._plan_cartesian_path_service.service_is_ready(): self._node.get_logger().warn( f"Service '{self._plan_cartesian_path_service.srv_name}' is not yet available. Better luck next time!" ) return None - res = self._plan_cartesian_path_service.call(self.__cartesian_path_request) - - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.solution.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None + return self._plan_cartesian_path_service.call_async(self.__cartesian_path_request) - def _send_goal_async_move_action( - self, wait_for_server_timeout_sec: Optional[float] = 1.0 - ): + def _send_goal_async_move_action(self): + self.__execution_mutex.acquire() stamp = self._node.get_clock().now().to_msg() self.__move_action_goal.request.workspace_parameters.header.stamp = stamp - if not self.__move_action_client.wait_for_server( - timeout_sec=wait_for_server_timeout_sec - ): + if not self.__move_action_client.service_is_ready(): self._node.get_logger().warn( f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" ) - self.__is_motion_requested = False return + self.__is_motion_requested = True self.__send_goal_future_move_action = self.__move_action_client.send_goal_async( goal=self.__move_action_goal, feedback_callback=None, @@ -1471,8 +1520,10 @@ def _send_goal_async_move_action( self.__send_goal_future_move_action.add_done_callback( self.__response_callback_move_action ) + self.__execution_mutex.release() def __response_callback_move_action(self, response): + self.__execution_mutex.acquire() goal_handle = response.result() if not goal_handle.accepted: self._node.get_logger().warn( @@ -1481,6 +1532,7 @@ def __response_callback_move_action(self, response): self.__is_motion_requested = False return + self.__execution_goal_handle = goal_handle self.__is_executing = True self.__is_motion_requested = False @@ -1488,8 +1540,10 @@ def __response_callback_move_action(self, response): self.__get_result_future_move_action.add_done_callback( self.__result_callback_move_action ) + self.__execution_mutex.release() def __result_callback_move_action(self, res): + self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: self._node.get_logger().error( f"Action '{self.__move_action_client._action_name}' was unsuccessful: {res.result().status}." @@ -1498,23 +1552,25 @@ def __result_callback_move_action(self, res): else: self.motion_suceeded = True + self.__last_error_code = res.result().result.error_code + + self.__execution_goal_handle = None self.__is_executing = False + self.__execution_mutex.release() def _send_goal_async_follow_joint_trajectory( self, goal: FollowJointTrajectory, - wait_for_server_timeout_sec: Optional[float] = 1.0, wait_until_response: bool = False, ): - if not self.__follow_joint_trajectory_action_client.wait_for_server( - timeout_sec=wait_for_server_timeout_sec - ): + self.__execution_mutex.acquire() + if not self.__follow_joint_trajectory_action_client.server_is_ready(): self._node.get_logger().warn( f"Action server '{self.__follow_joint_trajectory_action_client._action_name}' is not yet available. Better luck next time!" ) - self.__is_motion_requested = False return None + self.__is_motion_requested = True action_result = self.__follow_joint_trajectory_action_client.send_goal_async( goal=goal, feedback_callback=None, @@ -1525,17 +1581,20 @@ def _send_goal_async_follow_joint_trajectory( ) if wait_until_response: + self.__execution_mutex.release() self.__future_done_event.clear() action_result.add_done_callback( self.__response_callback_with_event_set_follow_joint_trajectory ) - self.__future_done_event.wait(timeout=wait_for_server_timeout_sec) + self.__future_done_event.wait() else: action_result.add_done_callback( self.__response_callback_follow_joint_trajectory ) + self.__execution_mutex.release() def __response_callback_follow_joint_trajectory(self, response): + self.__execution_mutex.acquire() goal_handle = response.result() if not goal_handle.accepted: self._node.get_logger().warn( @@ -1544,6 +1603,7 @@ def __response_callback_follow_joint_trajectory(self, response): self.__is_motion_requested = False return + self.__execution_goal_handle = goal_handle self.__is_executing = True self.__is_motion_requested = False @@ -1553,12 +1613,14 @@ def __response_callback_follow_joint_trajectory(self, response): self.__get_result_future_follow_joint_trajectory.add_done_callback( self.__result_callback_follow_joint_trajectory ) + self.__execution_mutex.release() def __response_callback_with_event_set_follow_joint_trajectory(self, response): self.__response_callback_follow_joint_trajectory(response) self.__future_done_event.set() def __result_callback_follow_joint_trajectory(self, res): + self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: self._node.get_logger().error( f"Action '{self.__follow_joint_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." @@ -1567,7 +1629,9 @@ def __result_callback_follow_joint_trajectory(self, res): else: self.motion_suceeded = True + self.__execution_goal_handle = None self.__is_executing = False + self.__execution_mutex.release() @classmethod def __init_move_action_goal( From 6b3b9c5cbecc947b7da51aeb0be026ea4af952bf Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Tue, 20 Jun 2023 15:32:19 -0700 Subject: [PATCH 03/18] Switch to ExecuteTrajectory action --- pymoveit2/moveit2.py | 74 ++++++++++++++++++++------------------------ 1 file changed, 34 insertions(+), 40 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 7fbd268..8a0302c 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -3,7 +3,7 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion -from moveit_msgs.action import MoveGroup +from moveit_msgs.action import MoveGroup, ExecuteTrajectory from moveit_msgs.msg import ( AttachedCollisionObject, CollisionObject, @@ -52,7 +52,6 @@ def __init__( group_name: str = "arm", execute_via_moveit: bool = False, callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "joint_trajectory_controller/follow_joint_trajectory", ): """ Construct an instance of `MoveIt2` interface. @@ -65,7 +64,6 @@ def __init__( ExecuteTrajectory action is employed otherwise together with a separate planning service client - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - - `follow_joint_trajectory_action_name` - Name of the action server for the controller """ self._node = node @@ -153,10 +151,10 @@ def __init__( self.__cartesian_path_request = GetCartesianPath.Request() # Create action client for trajectory execution - self.__follow_joint_trajectory_action_client = ActionClient( + self.__execute_trajectory_action_client = ActionClient( node=self._node, - action_type=FollowJointTrajectory, - action_name=follow_joint_trajectory_action_name, + action_type=ExecuteTrajectory, + action_name="execute_trajectory", goal_service_qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, @@ -625,17 +623,17 @@ def execute(self, joint_trajectory: JointTrajectory): ) return - follow_joint_trajectory_goal = init_follow_joint_trajectory_goal( + execute_trajectory_goal = init_execute_trajectory_goal( joint_trajectory=joint_trajectory ) - if follow_joint_trajectory_goal is None: + if execute_trajectory_goal is None: self._node.get_logger().warn( "Cannot execute motion because the provided/planned trajectory is invalid." ) return - self._send_goal_async_follow_joint_trajectory(goal=follow_joint_trajectory_goal) + self._send_goal_async_execute_trajectory(goal=execute_trajectory_goal) def wait_until_executed(self) -> bool: """ @@ -667,12 +665,12 @@ def reset_controller( joint_positions=joint_state, ) joint_trajectory = init_dummy_joint_trajectory_from_state(joint_state) - follow_joint_trajectory_goal = init_follow_joint_trajectory_goal( + execute_trajectory_goal = init_execute_trajectory_goal( joint_trajectory=joint_trajectory ) - self._send_goal_async_follow_joint_trajectory( - goal=follow_joint_trajectory_goal, + self._send_goal_async_execute_trajectory( + goal=execute_trajectory_goal, wait_until_response=sync, ) @@ -1558,47 +1556,47 @@ def __result_callback_move_action(self, res): self.__is_executing = False self.__execution_mutex.release() - def _send_goal_async_follow_joint_trajectory( + def _send_goal_async_execute_trajectory( self, - goal: FollowJointTrajectory, + goal: ExecuteTrajectory, wait_until_response: bool = False, ): self.__execution_mutex.acquire() - if not self.__follow_joint_trajectory_action_client.server_is_ready(): + if not self.__execute_trajectory_action_client.server_is_ready(): self._node.get_logger().warn( - f"Action server '{self.__follow_joint_trajectory_action_client._action_name}' is not yet available. Better luck next time!" + f"Action server '{self.__execute_trajectory_action_client._action_name}' is not yet available. Better luck next time!" ) return None self.__is_motion_requested = True - action_result = self.__follow_joint_trajectory_action_client.send_goal_async( + action_result = self.__execute_trajectory_action_client.send_goal_async( goal=goal, feedback_callback=None, ) action_result.add_done_callback( - self.__response_callback_follow_joint_trajectory + self.__response_callback_execute_trajectory ) if wait_until_response: self.__execution_mutex.release() self.__future_done_event.clear() action_result.add_done_callback( - self.__response_callback_with_event_set_follow_joint_trajectory + self.__response_callback_with_event_set_execute_trajectory ) self.__future_done_event.wait() else: action_result.add_done_callback( - self.__response_callback_follow_joint_trajectory + self.__response_callback_execute_trajectory ) self.__execution_mutex.release() - def __response_callback_follow_joint_trajectory(self, response): + def __response_callback_execute_trajectory(self, response): self.__execution_mutex.acquire() goal_handle = response.result() if not goal_handle.accepted: self._node.get_logger().warn( - f"Action '{self.__follow_joint_trajectory_action_client._action_name}' was rejected." + f"Action '{self.__execute_trajectory_action_client._action_name}' was rejected." ) self.__is_motion_requested = False return @@ -1607,28 +1605,30 @@ def __response_callback_follow_joint_trajectory(self, response): self.__is_executing = True self.__is_motion_requested = False - self.__get_result_future_follow_joint_trajectory = ( + self.__get_result_future_execute_trajectory = ( goal_handle.get_result_async() ) - self.__get_result_future_follow_joint_trajectory.add_done_callback( - self.__result_callback_follow_joint_trajectory + self.__get_result_future_execute_trajectory.add_done_callback( + self.__result_callback_execute_trajectory ) self.__execution_mutex.release() - def __response_callback_with_event_set_follow_joint_trajectory(self, response): - self.__response_callback_follow_joint_trajectory(response) + def __response_callback_with_event_set_execute_trajectory(self, response): + self.__response_callback_execute_trajectory(response) self.__future_done_event.set() - def __result_callback_follow_joint_trajectory(self, res): + def __result_callback_execute_trajectory(self, res): self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: self._node.get_logger().error( - f"Action '{self.__follow_joint_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." + f"Action '{self.__execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." ) self.motion_suceeded = False else: self.motion_suceeded = True + self.__last_error_code = res.result().result.error_code + self.__execution_goal_handle = None self.__is_executing = False self.__execution_mutex.release() @@ -1797,23 +1797,17 @@ def init_joint_state( return joint_state -def init_follow_joint_trajectory_goal( +def init_execute_trajectory_goal( joint_trajectory: JointTrajectory, -) -> Optional[FollowJointTrajectory.Goal]: +) -> Optional[ExecuteTrajectory.Goal]: if joint_trajectory is None: return None - follow_joint_trajectory_goal = FollowJointTrajectory.Goal() + execute_trajectory_goal = ExecuteTrajectory.Goal() - follow_joint_trajectory_goal.trajectory = joint_trajectory - # follow_joint_trajectory_goal.multi_dof_trajectory = "Ignored" - # follow_joint_trajectory_goal.path_tolerance = "Ignored" - # follow_joint_trajectory_goal.component_path_tolerance = "Ignored" - # follow_joint_trajectory_goal.goal_tolerance = "Ignored" - # follow_joint_trajectory_goal.component_goal_tolerance = "Ignored" - # follow_joint_trajectory_goal.goal_time_tolerance = "Ignored" + execute_trajectory_goal.trajectory.joint_trajectory = joint_trajectory - return follow_joint_trajectory_goal + return execute_trajectory_goal def init_dummy_joint_trajectory_from_state( From 7fcfac3d532b0edd92070b20681aee8e2cbe16eb Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Tue, 20 Jun 2023 17:57:08 -0700 Subject: [PATCH 04/18] [WIP] Goal cancellation is broken --- examples/ex_joint_goal.py | 13 +++- pymoveit2/__init__.py | 2 +- pymoveit2/moveit2.py | 145 ++++++++++++++++++-------------------- 3 files changed, 82 insertions(+), 78 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index 0f6fa19..8a955e4 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -10,7 +10,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 kinova @@ -65,7 +65,16 @@ def main(): # 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() + rate = node.create_rate(10) + while moveit2.query_state() != MoveIt2State.EXECUTING: + rate.sleep() + print("Current State: " + str(moveit2.query_state())) + print("Cancelling goal") + future = moveit2.cancel_execution() + print("Current State: " + str(moveit2.query_state())) + while not future.done(): + rate.sleep() + print("Cancellation result: " + str(future.result().return_code)) rclpy.shutdown() executor_thread.join() diff --git a/pymoveit2/__init__.py b/pymoveit2/__init__.py index c10e6d6..6c54a9d 100644 --- a/pymoveit2/__init__.py +++ b/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 diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 8a0302c..1d8097a 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -23,6 +23,7 @@ import rclpy from rclpy.action import ActionClient from rclpy.callback_groups import CallbackGroup +from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.qos import ( QoSDurabilityPolicy, @@ -36,6 +37,12 @@ from std_msgs.msg import Header from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from enum import Enum +class MoveIt2State(Enum): + IDLE = 0 + REQUESTING = 1 + EXECUTING = 2 + CANCELING = 3 class MoveIt2: """ @@ -136,6 +143,44 @@ def __init__( ) self.__kinematic_path_request = GetMotionPlan.Request() + # And create a separate action client for trajectory execution + self.__execute_trajectory_action_client = ActionClient( + node=self._node, + action_type=ExecuteTrajectory, + action_name="execute_trajectory", + goal_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + result_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + cancel_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + feedback_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + status_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) + # Create a separate service client for Cartesian planning self._plan_cartesian_path_service = self._node.create_client( srv_type=GetCartesianPath, @@ -150,44 +195,6 @@ def __init__( ) self.__cartesian_path_request = GetCartesianPath.Request() - # Create action client for trajectory execution - self.__execute_trajectory_action_client = ActionClient( - node=self._node, - action_type=ExecuteTrajectory, - action_name="execute_trajectory", - goal_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - result_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - cancel_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - feedback_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - status_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) - self.__collision_object_publisher = self._node.create_publisher( CollisionObject, "/collision_object", 10 ) @@ -229,32 +236,27 @@ def __init__( self.__future_done_event = threading.Event() #### Execution Polling Functions - from enum import Enum - class MoveIt2State(Enum): - IDLE = 0 - REQUESTING = 1 - EXECUTING = 2 - CANCELING = 3 - def query_state(self) -> MoveIt2State: with self.__execution_mutex: if self.__is_motion_requested: return MoveIt2State.REQUESTING elif self.__is_executing: + print(self.__execution_goal_handle.status) if self.__cancellation_future is None: return MoveIt2State.EXECUTING else: return MoveIt2State.CANCELING - else return MoveIt2State.IDLE + else: + return MoveIt2State.IDLE def cancel_execution(self) -> Optional[Future]: - with self.__execution_mutex: - if self.query_state() != MoveIt2State.EXECUTING: - self._node.get_logger().warn( - "Attempted to cancel without active goal." - ) - return None + if self.query_state() != MoveIt2State.EXECUTING: + self._node.get_logger().warn( + "Attempted to cancel without active goal." + ) + return None + with self.__execution_mutex: self.__cancellation_future = self.__execution_goal_handle.cancel_goal_async() self.__cancellation_future.add_done_callback( self.__cancel_callback @@ -266,12 +268,12 @@ def __cancel_callback(self, response): self.__execution_mutex.acquire() cancel_response = response.result() if len(cancel_response.goals_canceling) > 0: - self.__node.get_logger().info('Execution successfully canceled') + self._node.get_logger().info('Execution successfully canceled') self.__is_executing = False self.__execution_goal_handle = None self.__cancellation_future = None else: - self.__node.get_logger().error('Execution failed to cancel') + self._node.get_logger().error('Execution failed to cancel') self.__execution_mutex.release() def get_execution_future(self) -> Optional[Future]: @@ -457,12 +459,16 @@ def plan( """ Call plan_async and wait on future """ - future = self.plan(**locals()) + future = self.plan_async(**{key: value for key, value in locals().items() if key != 'self'}) if future is None: return None - rclpy.spin_until_future_complete(self._node, future) + # 10ms sleep + rate = self._node.create_rate(10) + while not future.done(): + rate.sleep() + res = future.result() # Cartesian @@ -1503,7 +1509,7 @@ def _send_goal_async_move_action(self): stamp = self._node.get_clock().now().to_msg() self.__move_action_goal.request.workspace_parameters.header.stamp = stamp - if not self.__move_action_client.service_is_ready(): + if not self.__move_action_client.server_is_ready(): self._node.get_logger().warn( f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" ) @@ -1518,6 +1524,7 @@ def _send_goal_async_move_action(self): self.__send_goal_future_move_action.add_done_callback( self.__response_callback_move_action ) + self.__execution_mutex.release() def __response_callback_move_action(self, response): @@ -1562,34 +1569,23 @@ def _send_goal_async_execute_trajectory( wait_until_response: bool = False, ): self.__execution_mutex.acquire() + if not self.__execute_trajectory_action_client.server_is_ready(): self._node.get_logger().warn( f"Action server '{self.__execute_trajectory_action_client._action_name}' is not yet available. Better luck next time!" ) - return None + return self.__is_motion_requested = True - action_result = self.__execute_trajectory_action_client.send_goal_async( + self.__send_goal_future_execute_trajectory = self.__execute_trajectory_action_client.send_goal_async( goal=goal, feedback_callback=None, ) - action_result.add_done_callback( + self.__send_goal_future_execute_trajectory.add_done_callback( self.__response_callback_execute_trajectory ) - - if wait_until_response: - self.__execution_mutex.release() - self.__future_done_event.clear() - action_result.add_done_callback( - self.__response_callback_with_event_set_execute_trajectory - ) - self.__future_done_event.wait() - else: - action_result.add_done_callback( - self.__response_callback_execute_trajectory - ) - self.__execution_mutex.release() + self.__execution_mutex.release() def __response_callback_execute_trajectory(self, response): self.__execution_mutex.acquire() @@ -1614,7 +1610,6 @@ def __response_callback_execute_trajectory(self, response): self.__execution_mutex.release() def __response_callback_with_event_set_execute_trajectory(self, response): - self.__response_callback_execute_trajectory(response) self.__future_done_event.set() def __result_callback_execute_trajectory(self, res): @@ -1628,7 +1623,7 @@ def __result_callback_execute_trajectory(self, res): self.motion_suceeded = True self.__last_error_code = res.result().result.error_code - + self.__execution_goal_handle = None self.__is_executing = False self.__execution_mutex.release() From ded6e87dbc8b5aa9d9007266de3e681a428dbed3 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Tue, 20 Jun 2023 18:38:28 -0700 Subject: [PATCH 05/18] Added cancellation via topic publication --- examples/ex_joint_goal.py | 10 ++++--- pymoveit2/moveit2.py | 57 +++++++++++++-------------------------- 2 files changed, 25 insertions(+), 42 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index 8a955e4..26ec8f3 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -66,15 +66,17 @@ def main(): node.get_logger().info(f"Moving to {{joint_positions: {list(joint_positions)}}}") moveit2.move_to_configuration(joint_positions) rate = node.create_rate(10) + print("Current State: " + str(moveit2.query_state())) while moveit2.query_state() != MoveIt2State.EXECUTING: rate.sleep() print("Current State: " + str(moveit2.query_state())) - print("Cancelling goal") - future = moveit2.cancel_execution() - print("Current State: " + str(moveit2.query_state())) + future = moveit2.get_execution_future() + #print("Cancelling goal") + #moveit2.cancel_execution() while not future.done(): rate.sleep() - print("Cancellation result: " + str(future.result().return_code)) + print("Result status: " + str(future.result().status)) + print("Result error code: " + str(future.result().result.error_code)) rclpy.shutdown() executor_thread.join() diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 1d8097a..7e90876 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -19,6 +19,7 @@ GetPositionFK, GetPositionIK, ) +from std_msgs.msg import String import rclpy from rclpy.action import ActionClient @@ -42,7 +43,6 @@ class MoveIt2State(Enum): IDLE = 0 REQUESTING = 1 EXECUTING = 2 - CANCELING = 3 class MoveIt2: """ @@ -57,7 +57,7 @@ def __init__( base_link_name: str, end_effector_name: str, group_name: str = "arm", - execute_via_moveit: bool = False, + use_move_action: bool = False, callback_group: Optional[CallbackGroup] = None, ): """ @@ -67,7 +67,7 @@ def __init__( - `base_link_name` - Name of the robot base link - `end_effector_name` - Name of the robot end effector - `group_name` - Name of the planning group for robot arm - - `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) + - `use_move_action` - Flag that enables execution via MoveGroup action (MoveIt 2) ExecuteTrajectory action is employed otherwise together with a separate planning service client - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) @@ -90,7 +90,7 @@ def __init__( callback_group=self._callback_group, ) - if execute_via_moveit: + if use_move_action: # Create action client for move action self.__move_action_client = ActionClient( node=self._node, @@ -202,6 +202,10 @@ def __init__( AttachedCollisionObject, "/attached_collision_object", 10 ) + self.__cancellation_pub = self._node.create_publisher( + String, "/trajectory_execution_event", 1 + ) + self.__joint_state_mutex = threading.Lock() self.__joint_state = None self.__new_joint_state_available = False @@ -214,7 +218,7 @@ def __init__( # Flag to determine whether to execute trajectories via Move Group Action, or rather by calling # the separate ExecuteTrajectory action # Applies to `move_to_pose()` and `move_to_configuraion()` - self.__execute_via_moveit = execute_via_moveit + self.__use_move_action = use_move_action # Store additional variables for later use self.__joint_names = joint_names @@ -226,7 +230,6 @@ def __init__( self.__is_motion_requested = False self.__is_executing = False self.motion_suceeded = False - self.__cancellation_future = None self.__execution_goal_handle = None self.__last_error_code = None self.__wait_until_executed_rate = self._node.create_rate(1000.0) @@ -241,44 +244,23 @@ def query_state(self) -> MoveIt2State: if self.__is_motion_requested: return MoveIt2State.REQUESTING elif self.__is_executing: - print(self.__execution_goal_handle.status) - if self.__cancellation_future is None: - return MoveIt2State.EXECUTING - else: - return MoveIt2State.CANCELING + return MoveIt2State.EXECUTING else: return MoveIt2State.IDLE - def cancel_execution(self) -> Optional[Future]: + def cancel_execution(self): if self.query_state() != MoveIt2State.EXECUTING: self._node.get_logger().warn( "Attempted to cancel without active goal." ) return None - with self.__execution_mutex: - self.__cancellation_future = self.__execution_goal_handle.cancel_goal_async() - self.__cancellation_future.add_done_callback( - self.__cancel_callback - ) - - return self.__cancellation_future - - def __cancel_callback(self, response): - self.__execution_mutex.acquire() - cancel_response = response.result() - if len(cancel_response.goals_canceling) > 0: - self._node.get_logger().info('Execution successfully canceled') - self.__is_executing = False - self.__execution_goal_handle = None - self.__cancellation_future = None - else: - self._node.get_logger().error('Execution failed to cancel') - self.__execution_mutex.release() + cancel_string = String() + cancel_string.data = "stop" + self.__cancellation_pub.publish(cancel_string) def get_execution_future(self) -> Optional[Future]: - with self.__execution_mutex: - if self.query_state() != MoveIt2State.EXECUTING: + if self.query_state() != MoveIt2State.EXECUTING: self._node.get_logger().warn( "Need active goal for future." ) @@ -345,7 +327,7 @@ def move_to_pose( pose=Pose(position=position, orientation=quat_xyzw), ) - if self.__execute_via_moveit and not cartesian: + if self.__use_move_action and not cartesian: if self.__is_motion_requested or self.__is_executing: self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." @@ -401,7 +383,7 @@ def move_to_configuration( passed in to internally use `set_joint_goal()` to define a goal during the call. """ - if self.__execute_via_moveit: + if self.__use_move_action: if self.__is_motion_requested or self.__is_executing: self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." @@ -1508,7 +1490,6 @@ def _send_goal_async_move_action(self): self.__execution_mutex.acquire() stamp = self._node.get_clock().now().to_msg() self.__move_action_goal.request.workspace_parameters.header.stamp = stamp - if not self.__move_action_client.server_is_ready(): self._node.get_logger().warn( f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" @@ -1550,7 +1531,7 @@ def __response_callback_move_action(self, response): def __result_callback_move_action(self, res): self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: - self._node.get_logger().error( + self._node.get_logger().warn( f"Action '{self.__move_action_client._action_name}' was unsuccessful: {res.result().status}." ) self.motion_suceeded = False @@ -1615,7 +1596,7 @@ def __response_callback_with_event_set_execute_trajectory(self, response): def __result_callback_execute_trajectory(self, res): self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: - self._node.get_logger().error( + self._node.get_logger().warn( f"Action '{self.__execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." ) self.motion_suceeded = False From 4cabc610b65d4d3247ef0ed793558d8c84579d75 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Tue, 20 Jun 2023 18:48:04 -0700 Subject: [PATCH 06/18] Full cancellation example --- examples/ex_joint_goal.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index 26ec8f3..f5823d4 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -71,8 +71,10 @@ def main(): rate.sleep() print("Current State: " + str(moveit2.query_state())) future = moveit2.get_execution_future() - #print("Cancelling goal") - #moveit2.cancel_execution() + onesec = node.create_rate(1) + onesec.sleep() + print("Cancelling goal") + moveit2.cancel_execution() while not future.done(): rate.sleep() print("Result status: " + str(future.result().status)) From 5ceeccb2e038b387fd42b733aa900cc0b388b945 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Tue, 20 Jun 2023 18:49:33 -0700 Subject: [PATCH 07/18] Need option for both move action and direct planning --- pymoveit2/moveit2.py | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 7e90876..de7c2a1 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -128,20 +128,20 @@ def __init__( ), callback_group=self._callback_group, ) - else: - # Otherwise create a separate service client for planning - self._plan_kinematic_path_service = self._node.create_client( - srv_type=GetMotionPlan, - srv_name="plan_kinematic_path", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=callback_group, - ) - self.__kinematic_path_request = GetMotionPlan.Request() + + # Also create a separate service client for planning + self._plan_kinematic_path_service = self._node.create_client( + srv_type=GetMotionPlan, + srv_name="plan_kinematic_path", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__kinematic_path_request = GetMotionPlan.Request() # And create a separate action client for trajectory execution self.__execute_trajectory_action_client = ActionClient( From 114e432d5759c50b5496110dbd062340af74a1f2 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 7 Jul 2023 18:14:15 -0700 Subject: [PATCH 08/18] Created get_trajectory, so users of plan_async can easily get the result --- pymoveit2/moveit2.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index de7c2a1..c111d50 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -217,7 +217,7 @@ def __init__( # Flag to determine whether to execute trajectories via Move Group Action, or rather by calling # the separate ExecuteTrajectory action - # Applies to `move_to_pose()` and `move_to_configuraion()` + # Applies to `move_to_pose()` and `move_to_configuration()` self.__use_move_action = use_move_action # Store additional variables for later use @@ -451,6 +451,19 @@ def plan( while not future.done(): rate.sleep() + return self.get_trajectory(future, cartesian=cartesian) + + def get_trajectory(self, future: Future, cartesian: bool = False) -> Optional[JointTrajectory]: + """ + Takes in a future returned by plan_async and returns the trajectory if the future is done + and planning was successful, else None. + """ + if (not future.done()): + self._node.get_logger().warn( + "Cannot get trajectory because future is not done." + ) + return None + res = future.result() # Cartesian From 72d58a2669828f1f2c712ca3e664830f9ab9e594 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 28 Jul 2023 14:42:15 -0700 Subject: [PATCH 09/18] Reset last error code before action execution Consider two cases, one where action server (either for execute or MoveGroup) is not available, and another where the action succeeds very fast. Both cases are currently indistinguishable from the client perspective, because they will request a goal, and then when they query the state it will be IDLE. This commit resolves that, because if the error code is set that means the action completed very fast, whereas if it is None that means the action did not complete. --- pymoveit2/moveit2.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index c111d50..055c799 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1509,6 +1509,7 @@ def _send_goal_async_move_action(self): ) return + self.__last_error_code = None self.__is_motion_requested = True self.__send_goal_future_move_action = self.__move_action_client.send_goal_async( goal=self.__move_action_goal, @@ -1570,6 +1571,7 @@ def _send_goal_async_execute_trajectory( ) return + self.__last_error_code = None self.__is_motion_requested = True self.__send_goal_future_execute_trajectory = self.__execute_trajectory_action_client.send_goal_async( goal=goal, From e03a070d19e7fe2317f1f721cda0488170e25a05 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 20 Dec 2023 10:55:58 -0800 Subject: [PATCH 10/18] Reverted to original example --- examples/ex_joint_goal.py | 40 ++++++++------------- pymoveit2/moveit2.py | 76 ++++++++++++++++++++------------------- 2 files changed, 53 insertions(+), 63 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index f5823d4..45e97b2 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -10,8 +10,8 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node -from pymoveit2 import MoveIt2, MoveIt2State -from pymoveit2.robots import kinova +from pymoveit2 import MoveIt2 +from pymoveit2.robots import panda def main(): @@ -24,12 +24,13 @@ def main(): node.declare_parameter( "joint_positions", [ - -1.47568, - 2.92779, - 1.00845, - -2.0847, - 1.43588, - 1.32575 + 0.0, + 0.0, + 0.0, + -0.7853981633974483, + 0.0, + 1.5707963267948966, + 0.7853981633974483, ], ) @@ -39,10 +40,10 @@ def main(): # Create MoveIt 2 interface moveit2 = MoveIt2( node=node, - joint_names=kinova.joint_names(), - base_link_name=kinova.base_link_name(), - end_effector_name="forkTip", - group_name=kinova.MOVE_GROUP_ARM, + joint_names=panda.joint_names(), + base_link_name=panda.base_link_name(), + end_effector_name=panda.end_effector_name(), + group_name=panda.MOVE_GROUP_ARM, callback_group=callback_group, ) @@ -65,20 +66,7 @@ def main(): # Move to joint configuration node.get_logger().info(f"Moving to {{joint_positions: {list(joint_positions)}}}") moveit2.move_to_configuration(joint_positions) - rate = node.create_rate(10) - print("Current State: " + str(moveit2.query_state())) - while moveit2.query_state() != MoveIt2State.EXECUTING: - rate.sleep() - print("Current State: " + str(moveit2.query_state())) - future = moveit2.get_execution_future() - onesec = node.create_rate(1) - onesec.sleep() - print("Cancelling goal") - moveit2.cancel_execution() - while not future.done(): - rate.sleep() - print("Result status: " + str(future.result().status)) - print("Result error code: " + str(future.result().result.error_code)) + moveit2.wait_until_executed() rclpy.shutdown() executor_thread.join() diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 055c799..b6e4917 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -143,43 +143,43 @@ def __init__( ) self.__kinematic_path_request = GetMotionPlan.Request() - # And create a separate action client for trajectory execution - self.__execute_trajectory_action_client = ActionClient( - node=self._node, - action_type=ExecuteTrajectory, - action_name="execute_trajectory", - goal_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - result_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - cancel_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - feedback_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - status_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) + # And create a separate action client for trajectory execution + self.__execute_trajectory_action_client = ActionClient( + node=self._node, + action_type=ExecuteTrajectory, + action_name="execute_trajectory", + goal_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + result_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + cancel_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + feedback_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + status_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) # Create a separate service client for Cartesian planning self._plan_cartesian_path_service = self._node.create_client( @@ -488,6 +488,7 @@ def get_trajectory(self, future: Future, cartesian: bool = False) -> Optional[Jo def plan_async( self, + pose: Optional[Union[PoseStamped, Pose]] = None, position: Optional[Union[Point, Tuple[float, float, float]]] = None, quat_xyzw: Optional[ Union[Quaternion, Tuple[float, float, float, float]] @@ -495,6 +496,7 @@ def plan_async( joint_positions: Optional[List[float]] = None, joint_names: Optional[List[str]] = None, frame_id: Optional[str] = None, + target_link: Optional[str] = None, tolerance_position: float = 0.001, tolerance_orientation: float = 0.001, tolerance_joint_position: float = 0.001, From 25b878f7cc4036a0b65755b8d23c9ac46ccc35d1 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 20 Dec 2023 12:00:17 -0800 Subject: [PATCH 11/18] Small fixes from rebase --- pymoveit2/moveit2.py | 186 ++++++++++++++++++++++++------------------- 1 file changed, 102 insertions(+), 84 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index b6e4917..c220eb7 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -21,10 +21,8 @@ ) from std_msgs.msg import String -import rclpy from rclpy.action import ActionClient from rclpy.callback_groups import CallbackGroup -from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.qos import ( QoSDurabilityPolicy, @@ -40,6 +38,14 @@ from enum import Enum class MoveIt2State(Enum): + """ + An enum the represents the current execution state of the MoveIt2 interface. + - IDLE: No motion is being requested or executed + - REQUESTING: Execution has been requested, but the request has not yet been + accepted. + - EXECUTING: Execution has been requested and accepted, and has not yet been + completed. + """ IDLE = 0 REQUESTING = 1 EXECUTING = 2 @@ -57,7 +63,8 @@ def __init__( base_link_name: str, end_effector_name: str, group_name: str = "arm", - use_move_action: bool = False, + use_move_group_action: bool = False, + ignore_new_calls_while_executing: bool = False, callback_group: Optional[CallbackGroup] = None, ): """ @@ -67,9 +74,9 @@ def __init__( - `base_link_name` - Name of the robot base link - `end_effector_name` - Name of the robot end effector - `group_name` - Name of the planning group for robot arm - - `use_move_action` - Flag that enables execution via MoveGroup action (MoveIt 2) - ExecuteTrajectory action is employed otherwise - together with a separate planning service client + - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) + ExecuteTrajectory action is employed otherwise + together with a separate planning service client - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) """ @@ -90,44 +97,43 @@ def __init__( callback_group=self._callback_group, ) - if use_move_action: - # Create action client for move action - self.__move_action_client = ActionClient( - node=self._node, - action_type=MoveGroup, - action_name="move_action", - goal_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - result_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - cancel_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - feedback_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - status_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) + # Create action client for move action + self.__move_action_client = ActionClient( + node=self._node, + action_type=MoveGroup, + action_name="move_action", + goal_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + result_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + cancel_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + feedback_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + status_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) # Also create a separate service client for planning self._plan_kinematic_path_service = self._node.create_client( @@ -143,7 +149,7 @@ def __init__( ) self.__kinematic_path_request = GetMotionPlan.Request() - # And create a separate action client for trajectory execution + # Create action client for trajectory execution self.__execute_trajectory_action_client = ActionClient( node=self._node, action_type=ExecuteTrajectory, @@ -218,7 +224,10 @@ def __init__( # Flag to determine whether to execute trajectories via Move Group Action, or rather by calling # the separate ExecuteTrajectory action # Applies to `move_to_pose()` and `move_to_configuration()` - self.__use_move_action = use_move_action + self.__use_move_group_action = use_move_group_action + + # Flag that determines whether a new goal can be sent while the previous one is being executed + self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing # Store additional variables for later use self.__joint_names = joint_names @@ -327,8 +336,11 @@ def move_to_pose( pose=Pose(position=position, orientation=quat_xyzw), ) - if self.__use_move_action and not cartesian: - if self.__is_motion_requested or self.__is_executing: + if self.__use_move_group_action and not cartesian: + if ( + self.__ignore_new_calls_while_executing and + (self.__is_motion_requested or self.__is_executing) + ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) @@ -383,8 +395,11 @@ def move_to_configuration( passed in to internally use `set_joint_goal()` to define a goal during the call. """ - if self.__use_move_action: - if self.__is_motion_requested or self.__is_executing: + if self.__use_move_group_action: + if ( + self.__ignore_new_calls_while_executing and + (self.__is_motion_requested or self.__is_executing) + ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) @@ -453,39 +468,6 @@ def plan( return self.get_trajectory(future, cartesian=cartesian) - def get_trajectory(self, future: Future, cartesian: bool = False) -> Optional[JointTrajectory]: - """ - Takes in a future returned by plan_async and returns the trajectory if the future is done - and planning was successful, else None. - """ - if (not future.done()): - self._node.get_logger().warn( - "Cannot get trajectory because future is not done." - ) - return None - - res = future.result() - - # Cartesian - if cartesian: - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.solution.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None - - # Else Kinematic - res = res.motion_plan_response - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.trajectory.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None - def plan_async( self, pose: Optional[Union[PoseStamped, Pose]] = None, @@ -615,12 +597,48 @@ def plan_async( return future + def get_trajectory(self, future: Future, cartesian: bool = False) -> Optional[JointTrajectory]: + """ + Takes in a future returned by plan_async and returns the trajectory if the future is done + and planning was successful, else None. + """ + if (not future.done()): + self._node.get_logger().warn( + "Cannot get trajectory because future is not done." + ) + return None + + res = future.result() + + # Cartesian + if cartesian: + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.solution.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + + # Else Kinematic + res = res.motion_plan_response + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.trajectory.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + def execute(self, joint_trajectory: JointTrajectory): """ Execute joint_trajectory by communicating directly with the controller. """ - if self.__is_motion_requested or self.__is_executing: + if ( + self.__ignore_new_calls_while_executing and + (self.__is_motion_requested or self.__is_executing) + ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) From 1d1a47cb989a19c1df6333f9c7ed126035364abb Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 20 Dec 2023 14:39:21 -0800 Subject: [PATCH 12/18] Update examples --- examples/ex_joint_goal.py | 43 ++++++++++++++++++++++++++++++++++++--- examples/ex_pose_goal.py | 39 +++++++++++++++++++++++++++++++++-- pymoveit2/moveit2.py | 38 +++++++++++++++++----------------- 3 files changed, 96 insertions(+), 24 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index 45e97b2..ef616e4 100755 --- a/examples/ex_joint_goal.py +++ b/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:=-1.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 negative, don't cancel. Only used if synchronous is False + node.declare_parameter("cancel_after_secs", -1.0) # Create callback group that allows execution of callbacks in parallel without restrictions callback_group = ReentrantCallbackGroup() @@ -58,15 +63,47 @@ 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() diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index 4a76569..b81cccd 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -10,7 +10,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 +24,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 negative, don't cancel. Only used if synchronous is False + node.declare_parameter("cancel_after_secs", -1.0) # Create callback group that allows execution of callbacks in parallel without restrictions callback_group = ReentrantCallbackGroup() @@ -53,13 +56,45 @@ 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() diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index c220eb7..58a3785 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -134,7 +134,7 @@ def __init__( ), callback_group=self._callback_group, ) - + # Also create a separate service client for planning self._plan_kinematic_path_service = self._node.create_client( srv_type=GetMotionPlan, @@ -149,6 +149,20 @@ def __init__( ) self.__kinematic_path_request = GetMotionPlan.Request() + # Create a separate service client for Cartesian planning + self._plan_cartesian_path_service = self._node.create_client( + srv_type=GetCartesianPath, + srv_name="compute_cartesian_path", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__cartesian_path_request = GetCartesianPath.Request() + # Create action client for trajectory execution self.__execute_trajectory_action_client = ActionClient( node=self._node, @@ -187,20 +201,6 @@ def __init__( callback_group=self._callback_group, ) - # Create a separate service client for Cartesian planning - self._plan_cartesian_path_service = self._node.create_client( - srv_type=GetCartesianPath, - srv_name="compute_cartesian_path", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=callback_group, - ) - self.__cartesian_path_request = GetCartesianPath.Request() - self.__collision_object_publisher = self._node.create_publisher( CollisionObject, "/collision_object", 10 ) @@ -635,10 +635,10 @@ def execute(self, joint_trajectory: JointTrajectory): Execute joint_trajectory by communicating directly with the controller. """ - if ( - self.__ignore_new_calls_while_executing and - (self.__is_motion_requested or self.__is_executing) - ): + if ( + self.__ignore_new_calls_while_executing and + (self.__is_motion_requested or self.__is_executing) + ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) From 10189f2bc06d7257409f7a61fba826ba2b39f7e1 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 20 Dec 2023 14:40:54 -0800 Subject: [PATCH 13/18] Update docstrings --- pymoveit2/moveit2.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 58a3785..2686bcb 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -77,6 +77,8 @@ def __init__( - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) ExecuteTrajectory action is employed otherwise together with a separate planning service client + - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories + while previous is still being executed - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) """ From 4f81bbb26b0b5867de0a61b32692dbb910d98fb2 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 20 Dec 2023 14:42:37 -0800 Subject: [PATCH 14/18] Update docstrings --- pymoveit2/moveit2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 2686bcb..409b51f 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -77,7 +77,7 @@ def __init__( - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) ExecuteTrajectory action is employed otherwise together with a separate planning service client - - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories + - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories while previous is still being executed - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) """ From d084e4ca09fae2e3520fc1d7b06a02ba5445bb7a Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 21 Dec 2023 11:21:09 -0800 Subject: [PATCH 15/18] Addressed PR comments --- examples/ex_joint_goal.py | 6 +++--- examples/ex_pose_goal.py | 8 +++++--- pymoveit2/moveit2.py | 25 +++++++++++++++++++++---- 3 files changed, 29 insertions(+), 10 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index ef616e4..8ac923f 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -36,8 +36,8 @@ def main(): ], ) node.declare_parameter("synchronous", True) - # If negative, don't cancel. Only used if synchronous is False - node.declare_parameter("cancel_after_secs", -1.0) + # 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() @@ -89,7 +89,7 @@ def main(): future = moveit2.get_execution_future() # Cancel the goal - if cancel_after_secs >= 0.0: + if cancel_after_secs > 0.0: # Sleep for the specified time sleep_time = node.create_rate(cancel_after_secs) sleep_time.sleep() diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index b81cccd..fa5d795 100755 --- a/examples/ex_pose_goal.py +++ b/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:=-1.0 """ from threading import Thread @@ -25,8 +27,8 @@ def main(): node.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0]) node.declare_parameter("cartesian", False) node.declare_parameter("synchronous", True) - # If negative, don't cancel. Only used if synchronous is False - node.declare_parameter("cancel_after_secs", -1.0) + # 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() @@ -80,7 +82,7 @@ def main(): future = moveit2.get_execution_future() # Cancel the goal - if cancel_after_secs >= 0.0: + if cancel_after_secs > 0.0: # Sleep for the specified time sleep_time = node.create_rate(cancel_after_secs) sleep_time.sleep() diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 409b51f..a59e26c 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -63,9 +63,11 @@ def __init__( base_link_name: str, end_effector_name: str, group_name: str = "arm", - use_move_group_action: bool = False, + execute_via_moveit: bool = False, ignore_new_calls_while_executing: bool = False, callback_group: Optional[CallbackGroup] = None, + follow_joint_trajectory_action_name: str = "joint_trajectory_controller/follow_joint_trajectory", + use_move_group_action: bool = False, ): """ Construct an instance of `MoveIt2` interface. @@ -74,17 +76,32 @@ def __init__( - `base_link_name` - Name of the robot base link - `end_effector_name` - Name of the robot end effector - `group_name` - Name of the planning group for robot arm - - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) - ExecuteTrajectory action is employed otherwise - together with a separate planning service client + - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) + FollowJointTrajectory action (controller) is employed otherwise + together with a separate planning service client - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories while previous is still being executed - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) + - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller + - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) + ExecuteTrajectory action is employed otherwise + together with a separate planning service client """ self._node = node self._callback_group = callback_group + # Check for deprecated parameters + if execute_via_moveit: + self._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 != "joint_trajectory_controller/follow_joint_trajectory": + self._node.get_logger().warn( + "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." + ) + # Create subscriber for current joint states self._node.create_subscription( msg_type=JointState, From f560d0cc18aa38a2db7fecdb1e6ce567570247d7 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 21 Dec 2023 11:33:57 -0800 Subject: [PATCH 16/18] Ran pre-commit hook --- examples/ex_joint_goal.py | 4 ++- examples/ex_pose_goal.py | 4 ++- pymoveit2/moveit2.py | 69 ++++++++++++++++++++------------------- 3 files changed, 42 insertions(+), 35 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index 8ac923f..e4f9661 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -68,7 +68,9 @@ def main(): 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 + 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)}}}") diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index fa5d795..eac1e3d 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -59,7 +59,9 @@ def main(): 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 + cancel_after_secs = ( + node.get_parameter("cancel_after_secs").get_parameter_value().double_value + ) # Move to pose node.get_logger().info( diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index a59e26c..1b568a7 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1,9 +1,10 @@ import threading +from enum import Enum from typing import List, Optional, Tuple, Union from action_msgs.msg import GoalStatus from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion -from moveit_msgs.action import MoveGroup, ExecuteTrajectory +from moveit_msgs.action import ExecuteTrajectory, MoveGroup from moveit_msgs.msg import ( AttachedCollisionObject, CollisionObject, @@ -19,8 +20,6 @@ GetPositionFK, GetPositionIK, ) -from std_msgs.msg import String - from rclpy.action import ActionClient from rclpy.callback_groups import CallbackGroup from rclpy.node import Node @@ -33,10 +32,10 @@ from rclpy.task import Future from sensor_msgs.msg import JointState from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive -from std_msgs.msg import Header +from std_msgs.msg import Header, String from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from enum import Enum + class MoveIt2State(Enum): """ An enum the represents the current execution state of the MoveIt2 interface. @@ -46,10 +45,12 @@ class MoveIt2State(Enum): - EXECUTING: Execution has been requested and accepted, and has not yet been completed. """ + IDLE = 0 REQUESTING = 1 EXECUTING = 2 + class MoveIt2: """ Python interface for MoveIt 2 that enables planning and execution of trajectories. @@ -97,7 +98,10 @@ def __init__( "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." ) use_move_group_action = True - if follow_joint_trajectory_action_name != "joint_trajectory_controller/follow_joint_trajectory": + if ( + follow_joint_trajectory_action_name + != "joint_trajectory_controller/follow_joint_trajectory" + ): self._node.get_logger().warn( "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." ) @@ -278,9 +282,7 @@ def query_state(self) -> MoveIt2State: def cancel_execution(self): if self.query_state() != MoveIt2State.EXECUTING: - self._node.get_logger().warn( - "Attempted to cancel without active goal." - ) + self._node.get_logger().warn("Attempted to cancel without active goal.") return None cancel_string = String() @@ -289,10 +291,8 @@ def cancel_execution(self): def get_execution_future(self) -> Optional[Future]: if self.query_state() != MoveIt2State.EXECUTING: - self._node.get_logger().warn( - "Need active goal for future." - ) - return None + self._node.get_logger().warn("Need active goal for future.") + return None return self.__execution_goal_handle.get_result_async() @@ -356,9 +356,8 @@ def move_to_pose( ) if self.__use_move_group_action and not cartesian: - if ( - self.__ignore_new_calls_while_executing and - (self.__is_motion_requested or self.__is_executing) + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." @@ -415,9 +414,8 @@ def move_to_configuration( """ if self.__use_move_group_action: - if ( - self.__ignore_new_calls_while_executing and - (self.__is_motion_requested or self.__is_executing) + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." @@ -475,7 +473,9 @@ def plan( """ Call plan_async and wait on future """ - future = self.plan_async(**{key: value for key, value in locals().items() if key != 'self'}) + future = self.plan_async( + **{key: value for key, value in locals().items() if key != "self"} + ) if future is None: return None @@ -616,12 +616,14 @@ def plan_async( return future - def get_trajectory(self, future: Future, cartesian: bool = False) -> Optional[JointTrajectory]: + def get_trajectory( + self, future: Future, cartesian: bool = False + ) -> Optional[JointTrajectory]: """ Takes in a future returned by plan_async and returns the trajectory if the future is done and planning was successful, else None. """ - if (not future.done()): + if not future.done(): self._node.get_logger().warn( "Cannot get trajectory because future is not done." ) @@ -638,7 +640,7 @@ def get_trajectory(self, future: Future, cartesian: bool = False) -> Optional[Jo f"Planning failed! Error code: {res.error_code.val}." ) return None - + # Else Kinematic res = res.motion_plan_response if MoveItErrorCodes.SUCCESS == res.error_code.val: @@ -654,9 +656,8 @@ def execute(self, joint_trajectory: JointTrajectory): Execute joint_trajectory by communicating directly with the controller. """ - if ( - self.__ignore_new_calls_while_executing and - (self.__is_motion_requested or self.__is_executing) + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." @@ -1536,7 +1537,9 @@ def _plan_cartesian_path( ) return None - return self._plan_cartesian_path_service.call_async(self.__cartesian_path_request) + return self._plan_cartesian_path_service.call_async( + self.__cartesian_path_request + ) def _send_goal_async_move_action(self): self.__execution_mutex.acquire() @@ -1612,9 +1615,11 @@ def _send_goal_async_execute_trajectory( self.__last_error_code = None self.__is_motion_requested = True - self.__send_goal_future_execute_trajectory = self.__execute_trajectory_action_client.send_goal_async( - goal=goal, - feedback_callback=None, + self.__send_goal_future_execute_trajectory = ( + self.__execute_trajectory_action_client.send_goal_async( + goal=goal, + feedback_callback=None, + ) ) self.__send_goal_future_execute_trajectory.add_done_callback( @@ -1636,9 +1641,7 @@ def __response_callback_execute_trajectory(self, response): self.__is_executing = True self.__is_motion_requested = False - self.__get_result_future_execute_trajectory = ( - goal_handle.get_result_async() - ) + self.__get_result_future_execute_trajectory = goal_handle.get_result_async() self.__get_result_future_execute_trajectory.add_done_callback( self.__result_callback_execute_trajectory ) From b4712e854039099d44b2f07fc5897559954b34ee Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Dec 2023 12:26:56 -0800 Subject: [PATCH 17/18] Addressed PR comments --- examples/ex_joint_goal.py | 2 +- examples/ex_pose_goal.py | 2 +- pymoveit2/moveit2.py | 19 ++++++++----------- 3 files changed, 10 insertions(+), 13 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index e4f9661..cece348 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -3,7 +3,7 @@ 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:=-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 diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index eac1e3d..cbe7a70 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -3,7 +3,7 @@ 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:=-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 diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 1b568a7..f86c6b0 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -67,7 +67,7 @@ def __init__( execute_via_moveit: bool = False, ignore_new_calls_while_executing: bool = False, callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "joint_trajectory_controller/follow_joint_trajectory", + follow_joint_trajectory_action_name: str = "DEPRECATED", use_move_group_action: bool = False, ): """ @@ -98,10 +98,7 @@ def __init__( "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." ) use_move_group_action = True - if ( - follow_joint_trajectory_action_name - != "joint_trajectory_controller/follow_joint_trajectory" - ): + if follow_joint_trajectory_action_name != "DEPRECATED": self._node.get_logger().warn( "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." ) @@ -187,7 +184,7 @@ def __init__( self.__cartesian_path_request = GetCartesianPath.Request() # Create action client for trajectory execution - self.__execute_trajectory_action_client = ActionClient( + self._execute_trajectory_action_client = ActionClient( node=self._node, action_type=ExecuteTrajectory, action_name="execute_trajectory", @@ -1607,16 +1604,16 @@ def _send_goal_async_execute_trajectory( ): self.__execution_mutex.acquire() - if not self.__execute_trajectory_action_client.server_is_ready(): + if not self._execute_trajectory_action_client.server_is_ready(): self._node.get_logger().warn( - f"Action server '{self.__execute_trajectory_action_client._action_name}' is not yet available. Better luck next time!" + f"Action server '{self._execute_trajectory_action_client._action_name}' is not yet available. Better luck next time!" ) return self.__last_error_code = None self.__is_motion_requested = True self.__send_goal_future_execute_trajectory = ( - self.__execute_trajectory_action_client.send_goal_async( + self._execute_trajectory_action_client.send_goal_async( goal=goal, feedback_callback=None, ) @@ -1632,7 +1629,7 @@ def __response_callback_execute_trajectory(self, response): goal_handle = response.result() if not goal_handle.accepted: self._node.get_logger().warn( - f"Action '{self.__execute_trajectory_action_client._action_name}' was rejected." + f"Action '{self._execute_trajectory_action_client._action_name}' was rejected." ) self.__is_motion_requested = False return @@ -1654,7 +1651,7 @@ def __result_callback_execute_trajectory(self, res): self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: self._node.get_logger().warn( - f"Action '{self.__execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." + f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." ) self.motion_suceeded = False else: From 6532e3ad3702939fe4c4c7a633b8872b6e66ee56 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Dec 2023 12:28:13 -0800 Subject: [PATCH 18/18] Fixed gripper interface --- examples/ex_gripper.py | 1 - pymoveit2/gripper_interface.py | 19 +++++++++++++++---- pymoveit2/moveit2_gripper.py | 24 +++++++++++++++++++----- 3 files changed, 34 insertions(+), 10 deletions(-) diff --git a/examples/ex_gripper.py b/examples/ex_gripper.py index 28961f7..2af8906 100755 --- a/examples/ex_gripper.py +++ b/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", ) diff --git a/pymoveit2/gripper_interface.py b/pymoveit2/gripper_interface.py index 7ce5b5f..0503805 100644 --- a/pymoveit2/gripper_interface.py +++ b/pymoveit2/gripper_interface.py @@ -26,14 +26,26 @@ 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, @@ -41,12 +53,11 @@ def __init__( 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 diff --git a/pymoveit2/moveit2_gripper.py b/pymoveit2/moveit2_gripper.py index 2d0b1fd..35fcdc1 100644 --- a/pymoveit2/moveit2_gripper.py +++ b/pymoveit2/moveit2_gripper.py @@ -25,7 +25,8 @@ def __init__( skip_planning: bool = False, skip_planning_fixed_motion_duration: float = 0.5, callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "gripper_trajectory_controller/follow_joint_trajectory", + follow_joint_trajectory_action_name: str = "DEPRECATED", + use_move_group_action: bool = False, ): """ Construct an instance of `MoveIt2Gripper` interface. @@ -34,7 +35,7 @@ def __init__( - `open_gripper_joint_positions` - Configuration of gripper joints when open - `closed_gripper_joint_positions` - Configuration of gripper joints when fully closed - `gripper_group_name` - Name of the planning group for robot gripper - - `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) + - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) FollowJointTrajectory action (controller) is employed otherwise together with a separate planning service client - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories @@ -45,19 +46,32 @@ def __init__( - `skip_planning_fixed_motion_duration` - Desired duration for the closing and opening motions when `skip_planning` mode is enabled. - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - - `follow_joint_trajectory_action_name` - Name of the action server for the controller + - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller + - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) + ExecuteTrajectory action is employed otherwise + together with a separate planning service client """ + # 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." + ) + super().__init__( node=node, joint_names=gripper_joint_names, base_link_name="", end_effector_name="", group_name=gripper_group_name, - execute_via_moveit=execute_via_moveit, ignore_new_calls_while_executing=ignore_new_calls_while_executing, callback_group=callback_group, - follow_joint_trajectory_action_name=follow_joint_trajectory_action_name, + use_move_group_action=use_move_group_action, ) self.__del_redundant_attributes()