Cleaned joint traj execute callback

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)