From 58e7a4f8381c55baf66fe40bbd0d4e7759679f7f Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Fri, 19 Jun 2020 02:25:28 -0400 Subject: [PATCH] Cleaned joint traj execute callback --- stretch_core/nodes/joint_trajectory_server.py | 198 +++++++++--------- 1 file changed, 96 insertions(+), 102 deletions(-) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index a8ac7d4..135e27d 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -12,8 +12,6 @@ from command_groups import HeadPanCommandGroup, HeadTiltCommandGroup, \ TelescopingCommandGroup, LiftCommandGroup, \ MobileBaseCommandGroup -GRIPPER_DEBUG = False - class JointTrajectoryAction: @@ -39,34 +37,27 @@ class JointTrajectoryAction: def execute_cb(self, goal): with self.node.robot_stop_lock: - if self.node.stop_the_robot: - # This trajectory callback has been called after a - # stop_the_robot service trigger that did not result - # in prempting a trajectory callback. Sufficient time - # is likely to have passed for the robot motors to - # have received their stop commands, so this - # trajectory command will be accepted. - - # Please note that it is possible that this trajectory - # command was sent before the stop_the_robot service - # trigger. - self.node.stop_the_robot = False - + # Escape stopped mode to execute trajectory + self.node.stop_the_robot = False self.node.robot_mode_rwlock.acquire_read() # For now, ignore goal time and configuration tolerances. - joint_names = goal.trajectory.joint_names - if self.node.trajectory_debug: - rospy.loginfo('New trajectory received with joint_names = {0}'.format(joint_names)) + commanded_joint_names = goal.trajectory.joint_names + rospy.loginfo('{0} joint_traj action: \ + New trajectory received with joint_names = {1}'.format(self.node.node_name, + commanded_joint_names)) ################################################### # Decide what to do based on the commanded joints. if self.node.use_lift: - command_groups = [self.telescoping_cg, self.lift_cg, self.mobile_base_cg, self.head_pan_cg, self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] + command_groups = [self.telescoping_cg, self.lift_cg, self.mobile_base_cg, + self.head_pan_cg, self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] else: - command_groups = [self.telescoping_cg, self.mobile_base_cg, self.head_pan_cg, self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] + command_groups = [self.telescoping_cg, self.mobile_base_cg, self.head_pan_cg, + self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] - updates = [c.update(joint_names, self.invalid_joints_callback, self.node.robot_mode) for c in command_groups] + updates = [c.update(commanded_joint_names, self.invalid_joints_callback, self.node.robot_mode) + for c in command_groups] if not all(updates): # The joint names violated at least one of the command # group's requirements. The command group should have @@ -74,30 +65,30 @@ class JointTrajectoryAction: self.node.robot_mode_rwlock.release_read() return - number_of_valid_joints = sum([c.get_num_valid_commands() for c in command_groups]) - - if number_of_valid_joints <= 0: - # Abort if no valid joints were received. - error_string = 'received a command without any valid joint names. Received joint names = ' + str(joint_names) - self.invalid_joints_callback(error_string) + num_valid_points = sum([c.get_num_valid_commands() for c in command_groups]) + if num_valid_points <= 0: + err_str = 'Received a command without any valid joint names. \ + Received joint names = {0}'.format(commanded_joint_names) + self.invalid_joints_callback(err_str) self.node.robot_mode_rwlock.release_read() return - - if len(joint_names) != number_of_valid_joints: - error_string = 'received {0} valid joints and {1} total joints. Received joint names = {2}'.format(number_of_valid_joints, len(joint_names), joint_names) - self.invalid_joints_callback(error_string) + elif num_valid_points != len(commanded_joint_names): + err_str = 'Received only {0} valid joints out of {1} total joints. \ + Received joint names = {2}'.format(num_valid_points, len(commanded_joint_names), + commanded_joint_names) + self.invalid_joints_callback(err_str) self.node.robot_mode_rwlock.release_read() return ################################################### - # Try to reach each of the goals in sequence until an error is - # detected or success is achieved. - for point_number, point in enumerate(goal.trajectory.points): - if self.node.trajectory_debug: - rospy.loginfo('position # {0} = {1}'.format(point_number, point.positions)) - - - valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.mobile_base_manipulation_origin) for c in command_groups] + # Try to reach each of the goals in sequence until + # an error is detected or success is achieved. + for pointi, point in enumerate(goal.trajectory.points): + rospy.logdebug('{0} joint_traj action: \ + target point #{1} = <{2}>'.format(self.node.node_name, pointi, point)) + + valid_goals = [c.set_goal(point, self.invalid_goal_callback, + self.node.mobile_base_manipulation_origin) for c in command_groups] if not all(valid_goals): # At least one of the goals violated the requirements # of a command group. Any violations should have been @@ -105,12 +96,9 @@ class JointTrajectoryAction: self.node.robot_mode_rwlock.release_read() return - # Attempt to reach the goal. - update_rate = rospy.Rate(15.0) - first_time = True incremental_commands_executed = False - + update_rate = rospy.Rate(15.0) goal_start_time = rospy.Time.now() while True: @@ -124,8 +112,10 @@ class JointTrajectoryAction: if self.node.use_lift: lift_error_m = self.lift_cg.update_execution(robot_status, self.node.backlash_state) - extension_error_m = self.telescoping_cg.update_execution(robot_status, self.node.backlash_state) - mobile_base_error_m, mobile_base_error_rad = self.mobile_base_cg.update_execution(robot_status, self.node.backlash_state) + extension_error_m = self.telescoping_cg.update_execution(robot_status, + self.node.backlash_state) + mobile_base_error_m, mobile_base_error_rad = \ + self.mobile_base_cg.update_execution(robot_status, self.node.backlash_state) self.head_pan_cg.update_execution(robot_status, self.node.backlash_state) self.head_tilt_cg.update_execution(robot_status, self.node.backlash_state) self.wrist_yaw_cg.update_execution(robot_status, self.node.backlash_state) @@ -134,62 +124,64 @@ class JointTrajectoryAction: # Check if a premption request has been received. with self.node.robot_stop_lock: if self.node.stop_the_robot or self.server.is_preempt_requested(): - if self.node.trajectory_debug: - rospy.loginfo('PREEMPTION REQUESTED, but not stopping current motions to allow smooth interpolation between old and new commands.') + rospy.logdebug('{0} joint_traj action: PREEMPTION REQUESTED, but not stopping \ + current motions to allow smooth interpolation between \ + old and new commands.'.format(self.node.node_name)) self.server.set_preempted() self.node.stop_the_robot = False self.node.robot_mode_rwlock.release_read() return - if not incremental_commands_executed: - translate = (mobile_base_error_m is not None) - rotate = (mobile_base_error_rad is not None) - if translate and rotate: - error_string = 'simultaneous translation and rotation of the mobile base requested. This is not allowed.' - self.invalid_goal_callback(error_string) - self.node.robot_mode_rwlock.release_read() - return - if translate: - self.node.robot.base.translate_by(mobile_base_error_m) - if rotate: - self.node.robot.base.rotate_by(mobile_base_error_rad) - - if self.telescoping_cg.extension_goal: - self.node.robot.arm.move_by(extension_error_m) - if extension_error_m < 0.0: - self.node.backlash_state['wrist_extension_retracted'] = True - else: - self.node.backlash_state['wrist_extension_retracted'] = False - - if self.node.use_lift: - if self.lift_cg.lift_goal: - self.node.robot.lift.move_by(lift_error_m) - - if self.head_pan_cg.joint_goal: - self.node.robot.head.move_by('head_pan', self.head_pan_cg.joint_error) - if self.head_pan_cg.joint_error > 0.0: - self.node.backlash_state['head_pan_looked_left'] = True - else: - self.node.backlash_state['head_pan_looked_left'] = False - - if self.head_tilt_cg.joint_goal: - self.node.robot.head.move_by('head_tilt', self.head_tilt_cg.joint_error) - if self.head_tilt_cg.joint_target > (self.node.head_tilt_backlash_transition_angle_rad + self.node.head_tilt_calibrated_offset_rad): - self.node.backlash_state['head_tilt_looking_up'] = True - else: - self.node.backlash_state['head_tilt_looking_up'] = False - - if self.wrist_yaw_cg.joint_goal: - self.node.robot.end_of_arm.move_to('wrist_yaw', self.wrist_yaw_cg.joint_target) - - if self.gripper_cg.gripper_joint_goal: - gripper_command = self.gripper_cg.goal_gripper_joint - if GRIPPER_DEBUG: - print('move_to stretch_gripper =', gripper_command) - self.node.robot.end_of_arm.move_to('stretch_gripper', gripper_command) - - self.node.robot.push_command() - incremental_commands_executed = True + if not incremental_commands_executed: + translate = (mobile_base_error_m is not None) + rotate = (mobile_base_error_rad is not None) + if translate and rotate: + err_str = 'Simultaneous translation and rotation of the mobile base requested. \ + This is not allowed.' + self.invalid_goal_callback(err_str) + self.node.robot_mode_rwlock.release_read() + return + if translate: + self.node.robot.base.translate_by(mobile_base_error_m) + if rotate: + self.node.robot.base.rotate_by(mobile_base_error_rad) + + if self.telescoping_cg.extension_goal: + self.node.robot.arm.move_by(extension_error_m) + if extension_error_m < 0.0: + self.node.backlash_state['wrist_extension_retracted'] = True + else: + self.node.backlash_state['wrist_extension_retracted'] = False + + if self.node.use_lift: + if self.lift_cg.lift_goal: + self.node.robot.lift.move_by(lift_error_m) + + if self.head_pan_cg.joint_goal: + self.node.robot.head.move_by('head_pan', self.head_pan_cg.joint_error) + if self.head_pan_cg.joint_error > 0.0: + self.node.backlash_state['head_pan_looked_left'] = True + else: + self.node.backlash_state['head_pan_looked_left'] = False + + if self.head_tilt_cg.joint_goal: + self.node.robot.head.move_by('head_tilt', self.head_tilt_cg.joint_error) + if self.head_tilt_cg.joint_target > (self.node.head_tilt_backlash_transition_angle_rad + self.node.head_tilt_calibrated_offset_rad): + self.node.backlash_state['head_tilt_looking_up'] = True + else: + self.node.backlash_state['head_tilt_looking_up'] = False + + if self.wrist_yaw_cg.joint_goal: + self.node.robot.end_of_arm.move_to('wrist_yaw', self.wrist_yaw_cg.joint_target) + + if self.gripper_cg.gripper_joint_goal: + gripper_command = self.gripper_cg.goal_gripper_joint + rospy.logdebug('{0} gripper debug: move_to stretch_gripper = \ + {1}'.format(self.node.node_name, gripper_command)) + self.node.robot.end_of_arm.move_to('stretch_gripper', gripper_command) + + self.node.robot.push_command() + incremental_commands_executed = True # Check if the goal positions have been reached. goals_reached = [c.goal_reached() for c in command_groups] @@ -199,13 +191,15 @@ class JointTrajectoryAction: break if (rospy.Time.now() - goal_start_time) > self.node.default_goal_timeout_duration: - error_string = 'time to execute the current goal point = {0} exceeded the default_goal_timeout = {1}'.format(point, self.node.default_goal_timeout_s) - self.goal_tolerance_violated_callback(error_string) + err_str = 'Time to execute the current goal point = <{0}> exceeded the \ + default_goal_timeout = {1}'.format(point, self.node.default_goal_timeout_s) + self.goal_tolerance_violated_callback(err_str) self.node.robot_mode_rwlock.release_read() return update_rate.sleep() + rospy.logdebug('{0} joint_traj action: Achieved target point.'.format(self.node.node_name)) # Currently not providing feedback. self.success_callback() @@ -213,25 +207,25 @@ class JointTrajectoryAction: return def invalid_joints_callback(self, err_str): - rospy.logerr('{0} action server: {1}'.format(self.node.node_name, err_str)) + rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) self.result.error_code = self.result.INVALID_JOINTS self.result.error_string = err_str self.server.set_aborted(self.result) def invalid_goal_callback(self, err_str): - rospy.logerr('{0} action server: {1}'.format(self.node.node_name, err_str)) + rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) self.result.error_code = self.result.INVALID_GOAL self.result.error_string = err_str self.result.set_aborted(self.result) def goal_tolerance_violated_callback(self, err_str): - rospy.logerr('{0} action server: {1}'.format(self.node.node_name, err_str)) + rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) self.result.error_code = self.result.GOAL_TOLERANCE_VIOLATED self.result.error_string = err_str self.server.set_aborted(self.result) def success_callback(self): - rospy.loginfo('{0} action server: Achieved all target points!'.format(self.node.node_name)) + rospy.loginfo('{0} joint_traj action: Achieved all target points!'.format(self.node.node_name)) self.result.error_code = self.result.SUCCESSFUL self.result.error_string = "Achieved all target points!" self.server.set_succeeded(self.result)