Generate Head Pan joint state info within command group

stretch_core/nodes/command_groups.py

@@ -151,6 +151,21 @@ class SimpleCommandGroup:
         """
         raise NotImplementedError
 
+    def joint_state(self, robot_status, **kwargs):
+        """Returns joint state of the robot
+
+        Parameters
+        ----------
+        robot_status: dict
+            robot's current status
+
+        Returns
+        -------
+        (float, float, float)
+            Current position, velocity, and effort
+        """
+        raise NotImplementedError
+
     def goal_reached(self):
         """Returns whether reached target point
 
@@ -166,39 +181,40 @@ class SimpleCommandGroup:
 
 
 class HeadPanCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_rad, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset, robot=None):
+    def __init__(self, range_rad, head_pan_calibrated_offset_rad, head_pan_calibrated_looked_left_offset_rad, robot=None):
         if range_rad is None and robot is not None:
             range_ticks = robot.head.motors['head_pan'].params['range_t']
             range_rad = (robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[1]),
                          robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[0]))
         SimpleCommandGroup.__init__(self, 'joint_head_pan', range_rad, acceptable_joint_error=0.15)
-        self.head_pan_calibrated_offset = head_pan_calibrated_offset
-        self.head_pan_calibrated_looked_left_offset = head_pan_calibrated_looked_left_offset
+        self.calibrated_offset_rad = head_pan_calibrated_offset_rad
+        self.calibrated_looked_left_offset_rad = head_pan_calibrated_looked_left_offset_rad
+        self.looked_left = False
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
-            _, pan_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
+            _, pan_error = self.update_execution(robot_status)
             robot.head.move_by('head_pan', pan_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
-            if pan_error > 0.0:
-                kwargs['backlash_state']['head_pan_looked_left'] = True
-            else:
-                kwargs['backlash_state']['head_pan_looked_left'] = False
+            self.looked_left = True if pan_error > 0.0 else False
 
     def update_execution(self, robot_status, **kwargs):
         self.error = None
-        backlash_state = kwargs['backlash_state']
         if self.active:
-            if backlash_state['head_pan_looked_left']:
-                pan_backlash_correction = self.head_pan_calibrated_looked_left_offset
-            else:
-                pan_backlash_correction = 0.0
-            pan_current = robot_status['head']['head_pan']['pos'] + \
-                          self.head_pan_calibrated_offset + pan_backlash_correction
+            pan_backlash_correction = self.calibrated_looked_left_offset_rad if self.looked_left else 0.0
+            pan_current = robot_status['head']['head_pan']['pos'] + self.calibrated_offset_rad + pan_backlash_correction
             self.error = self.goal['position'] - pan_current
             return self.name, self.error
 
         return None
 
+    def joint_state(self, robot_status, **kwargs):
+        pan_status = robot_status['head']['head_pan']
+        pan_backlash_correction = self.calibrated_looked_left_offset_rad if self.looked_left else 0.0
+        pos = pan_status['pos'] + self.calibrated_offset_rad + pan_backlash_correction
+        vel = pan_status['vel']
+        effort = pan_status['effort']
+        return (pos, vel, effort)
+
 
 class HeadTiltCommandGroup(SimpleCommandGroup):
     def __init__(self, range_rad, head_tilt_calibrated_offset,

stretch_core/nodes/joint_trajectory_server.py

@@ -72,11 +72,9 @@ class JointTrajectoryAction:
 
         ###################################################
         # Decide what to do based on the commanded joints.
-        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]
         updates = [c.update(commanded_joint_names, self.invalid_joints_callback,
                    robot_mode=self.node.robot_mode)
-                   for c in command_groups]
+                   for c in self.command_groups]
         if not all(updates):
             # The joint names violated at least one of the command
             # group's requirements. The command group should have
@@ -84,7 +82,7 @@ class JointTrajectoryAction:
             self.node.robot_mode_rwlock.release_read()
             return
 
-        num_valid_points = sum([c.get_num_valid_commands() for c in command_groups])
+        num_valid_points = sum([c.get_num_valid_commands() for c in self.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)
@@ -107,7 +105,7 @@ class JointTrajectoryAction:
 
             valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal,
                                       manipulation_origin=self.node.mobile_base_manipulation_origin)
-                           for c in command_groups]
+                           for c in self.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
@@ -117,10 +115,10 @@ class JointTrajectoryAction:
 
             robot_status = self.node.robot.get_status() # uses lock held by robot
             [c.init_execution(self.node.robot, robot_status, backlash_state=self.node.backlash_state)
-             for c in command_groups]
+             for c in self.command_groups]
             self.node.robot.push_command()
 
-            goals_reached = [c.goal_reached() for c in command_groups]
+            goals_reached = [c.goal_reached() for c in self.command_groups]
             update_rate = rospy.Rate(15.0)
             goal_start_time = rospy.Time.now()
 
@@ -146,13 +144,13 @@ class JointTrajectoryAction:
                 robot_status = self.node.robot.get_status()
                 named_errors = [c.update_execution(robot_status, success_callback=self.success_callback,
                                                    backlash_state=self.node.backlash_state)
-                                for c in command_groups]
+                                for c in self.command_groups]
                 if any(ret == True for ret in named_errors):
                     self.node.robot_mode_rwlock.release_read()
                     return
 
                 self.feedback_callback(commanded_joint_names, point, named_errors)
-                goals_reached = [c.goal_reached() for c in command_groups]
+                goals_reached = [c.goal_reached() for c in self.command_groups]
                 update_rate.sleep()
 
             rospy.logdebug("{0} joint_traj action: Achieved target point.".format(self.node.node_name))

stretch_core/nodes/stretch_driver

@@ -171,18 +171,8 @@ class StretchBodyNode:
                 print('gripper_finger_rad =', gripper_finger_rad)
                 print('-----------------------')
 
-        if self.use_robotis_head: 
-            # assign relevant head pan status to variables
-            head_pan_status = robot_status['head']['head_pan']
-            if self.backlash_state['head_pan_looked_left']:
-                pan_backlash_correction = self.head_pan_calibrated_looked_left_offset_rad 
-            else:
-                pan_backlash_correction = 0.0
-            if BACKLASH_DEBUG: 
-                print('pan_backlash_correction =', pan_backlash_correction)
-            head_pan_rad = head_pan_status['pos'] + self.head_pan_calibrated_offset_rad + pan_backlash_correction
-            head_pan_vel = head_pan_status['vel']
-            head_pan_effort = head_pan_status['effort']
+        if self.use_robotis_head:
+            head_pan_rad, head_pan_vel, head_pan_effort = self.joint_trajectory_action.command_groups[0].joint_state(robot_status)
 
             # assign relevant head tilt status to variables
             head_tilt_status = robot_status['head']['head_tilt']