From 95955215413f85429d26efc5c27e638a4a5f55c8 Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Wed, 8 Feb 2023 16:36:55 +0100 Subject: [PATCH] handling cancellations, goal conflicts and adding loggers (2) (#48) * handling goal cancel * reverting back the changes * removing unwanted variable * handling goals more elegantly * more minors for goal handle * clearing buffer at the sdk level for proper handling of new goal * handle cancellations * adding additional loggers * some more loggers --- .../elite_arm_driver/elite_arm_kinematics.py | 4 +- .../elite_arm_driver/elite_arm_move.py | 2 +- .../elite_arm_driver/elite_arm_set_io.py | 2 +- .../elite_arm_trajectory_action.py | 62 +++++++++++++------ .../elite_arm_driver/elite_state_publisher.py | 2 +- 5 files changed, 49 insertions(+), 23 deletions(-) diff --git a/elite_arm_driver/elite_arm_driver/elite_arm_kinematics.py b/elite_arm_driver/elite_arm_driver/elite_arm_kinematics.py index e086029..5fc2a13 100644 --- a/elite_arm_driver/elite_arm_driver/elite_arm_kinematics.py +++ b/elite_arm_driver/elite_arm_driver/elite_arm_kinematics.py @@ -6,7 +6,7 @@ class EliteArmKinematics(): def __init__(self): - print("Listening for kinematics request") + self.get_logger().info("Listening for kinematics request") self.fk_srv = self.create_service(ForwardKinematic, 'forward_kinematics', self.forward_kinematics_cb) self.ik_srv = self.create_service(InverseKinematic, 'inverse_kinematics', self.inverse_kinematics_cb) @@ -35,7 +35,7 @@ def inverse_kinematics_cb(self, request, response): target_point.append(math.degrees(angle)) result = self.elite_robot.get_inverse_kinematic( - target_point, request.ref_joint,unit_type=0) + target_point, request.ref_joint, unit_type=0) if type(result) == tuple: response.result = False else: diff --git a/elite_arm_driver/elite_arm_driver/elite_arm_move.py b/elite_arm_driver/elite_arm_driver/elite_arm_move.py index c1aac6b..f0c2641 100644 --- a/elite_arm_driver/elite_arm_driver/elite_arm_move.py +++ b/elite_arm_driver/elite_arm_driver/elite_arm_move.py @@ -4,7 +4,7 @@ class EliteArmMove: def __init__(self, ): - print("ready to move arm") + self.get_logger().info("ready to move arm") self.cart_mov_srv = self.create_service(CartMove, 'cart_move_server', self.cart_move_cb) self.joint_mov_srv = self.create_service(JointMove, 'joint_move_server', self.joint_move_cb) self.stop_mov_srv = self.create_service(StopMove, 'stop_move_server', self.stop_move_cb) diff --git a/elite_arm_driver/elite_arm_driver/elite_arm_set_io.py b/elite_arm_driver/elite_arm_driver/elite_arm_set_io.py index 893ff5a..103d96e 100644 --- a/elite_arm_driver/elite_arm_driver/elite_arm_set_io.py +++ b/elite_arm_driver/elite_arm_driver/elite_arm_set_io.py @@ -4,7 +4,7 @@ class EliteArmSetIO(): def __init__(self): - print("starting elite arm set IO") + self.get_logger().info("starting elite arm set IO") self.digital_io_srv = self.create_service(SetIO, 'set_digital_io', self.digital_io_cb) self.analog_io_srv = self.create_service(SetAnalogIO, 'set_analog_io', self.analog_io_cb) diff --git a/elite_arm_driver/elite_arm_driver/elite_arm_trajectory_action.py b/elite_arm_driver/elite_arm_driver/elite_arm_trajectory_action.py index dace5b9..6476a91 100644 --- a/elite_arm_driver/elite_arm_driver/elite_arm_trajectory_action.py +++ b/elite_arm_driver/elite_arm_driver/elite_arm_trajectory_action.py @@ -13,29 +13,44 @@ class EliteArmTrajectoryAction(): def __init__(self, ): - print("starting elite arm trajectory action") + self.get_logger().info("starting elite arm trajectory action") self.action_server = ActionServer( self, FollowJointTrajectory, 'arm_controller/follow_joint_trajectory', execute_callback=self.execute_callback, - callback_group=MutuallyExclusiveCallbackGroup(), + callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback) + cancel_callback=self.cancel_callback, + handle_accepted_callback=self.handle_accepted_callback) self.interpolate_sample_time = 0.008 self.stop_mov_cli = self.create_client(StopMove, 'stop_move_server') + self.goal_handle_ = None - def goal_callback(self, goal_handle): + def goal_callback(self, goal_): self.get_logger().info('Goal request recieved') - self.goal = goal_handle + self.goal = goal_ return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): # Accepts or rejects a client request to cancel an action self.get_logger().info("Stopping initiated") self._stop_move() + self.goal_handle = None return CancelResponse.ACCEPT + def handle_accepted_callback(self, goal_handle): + # Takes care of multiple goal requests + if self.goal_handle_ != None: + if self.goal_handle_.is_active: + self.get_logger().info("Stopping the executing goal") + self._stop_move() + self.goal_handle_.canceled() + + self.goal_handle_ = goal_handle + self.get_logger().info("Executing the new goal") + self.goal_handle_.execute() + def execute_callback(self, goal_handle): points = self.goal.trajectory.points joints = [] @@ -59,26 +74,39 @@ def execute_callback(self, goal_handle): joint.append(0.0) joint.append(0.0) - is_blocking = True - self.elite_robot.TT_init(t=time_stamp[1]*1000) last_joint = [] for i in range(len(time_stamp)): + # check if cancellation is requested + if goal_handle.is_cancel_requested: + goal_handle.canceled() + self.get_logger().info("Returning stop from the execution") + result = FollowJointTrajectory.Result() + result.error_code = result.SUCCESSFUL + result.error_string = "Cancelled" + + self.goal_handle_ = None + self.elite_robot.TT_clear_buff() + return result + temp_joint = joint[i*8:i*8+6] self.elite_robot.TT_add_joint(temp_joint) last_joint = temp_joint - if is_blocking: - while 1: - time.sleep(0.1) - self.elite_robot:EC - current_joint = [round(i,1) for i in self.elite_robot.current_joint] - goal_joint = [round(i,1) for i in last_joint] - if (current_joint == goal_joint): - self.elite_robot.TT_clear_buff() - break + + while 1: + time.sleep(0.1) + self.elite_robot:EC + current_joint = [round(i,1) for i in self.elite_robot.current_joint] + goal_joint = [round(i,1) for i in last_joint] + if (current_joint == goal_joint): + self.elite_robot.TT_clear_buff() + break goal_handle.succeed() + # reset the global goal_handle + self.goal_handle_ = None + # ToDo: Handle errors and failures result = FollowJointTrajectory.Result() result.error_code = result.SUCCESSFUL @@ -88,8 +116,6 @@ def execute_callback(self, goal_handle): return result - - def _stop_move(self) -> bool: request = StopMove.Request() self.future = self.stop_mov_cli.call_async(request) diff --git a/elite_arm_driver/elite_arm_driver/elite_state_publisher.py b/elite_arm_driver/elite_arm_driver/elite_state_publisher.py index f2f67c5..3f9e63e 100644 --- a/elite_arm_driver/elite_arm_driver/elite_state_publisher.py +++ b/elite_arm_driver/elite_arm_driver/elite_state_publisher.py @@ -7,7 +7,7 @@ class EliteStatePublisher(): def __init__(self): - print("starting elite state publishers") + self.get_logger().info("starting elite state publishers") # Publish joint states self.joint_state_publisher = self.create_publisher(JointState, "joint_states", 10) # Todo Ros parameter