Begin decoupling keyboard_teleop from hardcoded joints

hello_helpers/src/hello_helpers/hello_misc.py

@@ -67,9 +67,10 @@ def get_left_finger_state(joint_states):
     return [left_finger_position, left_finger_velocity, left_finger_effort]
 
 class HelloNode:
-    def __init__(self):
+    def __init__(self, verbose=True):
         self.joint_state = None
         self.point_cloud = None
+        self.verbose = verbose
 
     def joint_states_callback(self, joint_state):
         self.joint_state = joint_state
@@ -139,7 +140,8 @@ class HelloNode:
     def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
         rospy.init_node(node_name)
         self.node_name = rospy.get_name()
-        rospy.loginfo("{0} started".format(self.node_name))
+        if self.verbose:
+            rospy.loginfo("{0} started".format(self.node_name))
 
         self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
         server_reached = self.trajectory_client.wait_for_server(timeout=rospy.Duration(60.0))
@@ -154,7 +156,8 @@ class HelloNode:
         self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1)
 
         rospy.wait_for_service('/stop_the_robot')
-        rospy.loginfo('Node ' + self.node_name + ' connected to /stop_the_robot service.')
+        if self.verbose:
+            rospy.loginfo('Node ' + self.node_name + ' connected to /stop_the_robot service.')
         self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger)
         
         if wait_for_first_pointcloud:

stretch_core/launch/keyboard_teleop.launch

@@ -5,9 +5,9 @@
   <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
   <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
   <!-- -->
-    
+
   <!-- KEYBOARD TELEOP -->
   <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args=''/>
   <!-- -->
-    
+
 </launch>

stretch_core/launch/stretch_driver.launch

@@ -1,5 +1,5 @@
 <launch>
-  
+
   <param name="robot_description" textfile="$(find stretch_description)/urdf/stretch.urdf" />
   <arg name="calibrated_controller_yaml_file" value="$(find stretch_core)/config/controller_calibration_head.yaml"/>
 
@@ -25,7 +25,7 @@
     </rosparam>
   </node>
   -->
-  
+
   <node
       name="robot_state_publisher"
       pkg="robot_state_publisher"
@@ -40,5 +40,5 @@
     <remap from="joint_states" to="/stretch/joint_states" />
     <param name="controller_calibration_file" type="string" value="$(arg calibrated_controller_yaml_file)"/>
   </node>
-  
+
 </launch>

stretch_core/nodes/command_groups.py

@@ -125,7 +125,7 @@ class GripperCommandGroup(SimpleCommandGroup):
                                     self.gripper_conversion.robotis_to_aperture(range_robotis[1]))
             self.range_finger_rad = (self.gripper_conversion.robotis_to_finger(range_robotis[0]),
                                     self.gripper_conversion.robotis_to_finger(range_robotis[1]))
-        SimpleCommandGroup.__init__(self, 'joint_gripper', None, acceptable_joint_error=1.0)
+        SimpleCommandGroup.__init__(self, 'gripper_aperture', None, acceptable_joint_error=1.0)
         self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
 
     def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):

stretch_core/nodes/joint_trajectory_server.py

@@ -70,6 +70,9 @@ class JointTrajectoryAction:
                     endofarm_device = getattr(importlib.import_module(module_name), class_name)(None, self.node.robot)
                     self.command_groups.append(endofarm_device)
 
+        # Publish list of active joint to param sever
+        rospy.set_param('/stretch_controller/follow_joint_trajectory/joints', [cg.name for cg in self.command_groups])
+
     def execute_cb(self, goal):
         with self.node.robot_stop_lock:
             # Escape stopped mode to execute trajectory

stretch_core/nodes/keyboard_teleop

@@ -24,6 +24,8 @@ class GetKeyboardCommands:
         self.grasp_object_on = grasp_object_on
         self.deliver_object_on = deliver_object_on
 
+        self.joints = ['joint_lift', 'joint_mobile_base_translation', 'wrist_extension', 'joint_head_pan', 'joint_head_tilt', 'joint_wrist_yaw', 'gripper_aperture']
+
         self.step_size = 'medium'
         # self.persistent_command_count = 0
         # self.prev_persistent_c = None
@@ -50,39 +52,64 @@ class GetKeyboardCommands:
             deltas = {'rad': self.big_rad, 'translate': self.big_translate} 
         return deltas
 
+    def set_active_joints(self, joints):
+        self.joints = joints
+
     def print_commands(self):
-        print('---------- KEYBOARD TELEOP MENU -----------')
-        print('                                           ')
-        print('              i HEAD UP                    ')
-        print(' j HEAD LEFT            l HEAD RIGHT       ')
-        print('              , HEAD DOWN                  ')
-        print('                                           ')
-        print('                                           ')
-        print(' 7 BASE ROTATE LEFT     9 BASE ROTATE RIGHT')
-        print(' home                   page-up            ')
-        print('                                           ')
-        print('                                           ')
-        print('              8 LIFT UP                    ')
-        print('              up-arrow                     ')
-        print(' 4 BASE FORWARD         6 BASE BACK        ')
-        print(' left-arrow             right-arrow        ')
-        print('              2 LIFT DOWN                  ')
-        print('              down-arrow                   ')
-        print('                                           ')
-        print('                                           ')
-        print('              w ARM OUT                    ')
-        print(' a WRIST FORWARD        d WRIST BACK       ')
-        print('              x ARM IN                     ')
-        print('                                           ')
-        print('                                           ')
-        print('              5 GRIPPER CLOSE              ')
-        print('              0 GRIPPER OPEN               ')
-        print('                                           ')
-        print('  step size:  b BIG, m MEDIUM, s SMALL     ')
-        print('                                           ')
-        print('              q QUIT                       ')
-        print('                                           ')
-        print('-------------------------------------------')
+        print('|{:-^43}|'.format(' KEYBOARD TELEOP MENU '))
+        print('|{:^43}|'.format(''))
+
+        # head joints
+        if 'joint_head_tilt' in self.joints:
+            print('|{:^43}|'.format('i HEAD UP'))
+        if 'joint_head_pan' in self.joints:
+            print('|{:^22}{:^21}|'.format('j HEAD LEFT', 'l HEAD RIGHT'))
+        if 'joint_head_tilt' in self.joints:
+            print('|{:^43}|'.format(', HEAD DOWN'))
+
+        print('|{:^43}|'.format(''))
+        print('|{:^43}|'.format(''))
+
+        print('| 7 BASE ROTATE LEFT     9 BASE ROTATE RIGHT|')
+        print('| home                   page-up            |')
+
+        print('|{:^43}|'.format(''))
+        print('|{:^43}|'.format(''))
+
+        # base and lift joints
+        if 'joint_lift' in self.joints:
+            print('|{:^43}|'.format('8 LIFT UP'))
+            print('|{:^43}|'.format('up-arrow'))
+        print('| 4 BASE FORWARD         6 BASE BACK        |')
+        print('| left-arrow             right-arrow        |')
+        if 'joint_lift' in self.joints:
+            print('|{:^43}|'.format('2 LIFT DOWN'))
+            print('|{:^43}|'.format('down-arrow'))
+
+        print('|{:^43}|'.format(''))
+        print('|{:^43}|'.format(''))
+
+        # arm and wrist yaw joints
+        if 'joint_lift' in self.joints:
+            print('|{:^43}|'.format('w ARM OUT'))
+        if 'joint_wrist_yaw' in self.joints:
+            print('|{:^22}{:^21}|'.format('a WRIST FORWARD', 'd WRIST BACK'))
+        if 'joint_lift' in self.joints:
+            print('|{:^43}|'.format('x ARM IN'))
+
+        print('|{:^43}|'.format(''))
+        print('|{:^43}|'.format(''))
+
+        # gripper joint
+        if 'gripper_aperture' in self.joints:
+            print('|{:^43}|'.format('5 GRIPPER CLOSE'))
+            print('|{:^43}|'.format('0 GRIPPER OPEN'))
+
+        print('|{:^43}|'.format(''))
+        print('|{:^43}|'.format('step size:  b BIG, m MEDIUM, s SMALL'))
+        print('|{:^43}|'.format('q QUIT'))
+        print('|{:^43}|'.format(''))
+        print('|{:-^43}|'.format(''))
 
     def get_command(self, node):
         command = None
@@ -312,8 +339,10 @@ class KeyboardTeleopNode(hm.HelloNode):
 
     def main(self):
         hm.HelloNode.main(self, 'keyboard_teleop', 'keyboard_teleop', wait_for_first_pointcloud=False)
+        if rospy.has_param("/stretch_controller/follow_joint_trajectory/joints"):
+            self.keys.set_active_joints(rospy.get_param("/stretch_controller/follow_joint_trajectory/joints"))
 
-        if self.mapping_on: 
+        if self.mapping_on:
             rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_head_scan.')
 
             rospy.wait_for_service('/funmap/trigger_head_scan')
@@ -358,7 +387,6 @@ class KeyboardTeleopNode(hm.HelloNode):
             rospy.loginfo('Node ' + self.node_name + ' connected to /open_drawer/trigger_open_drawer_up.')
             self.trigger_open_drawer_up_service = rospy.ServiceProxy('/open_drawer/trigger_open_drawer_up', Trigger)
 
-            
         if self.clean_surface_on:
             rospy.wait_for_service('/clean_surface/trigger_clean_surface')
             rospy.loginfo('Node ' + self.node_name + ' connected to /clean_surface/trigger_clean_surface.')
@@ -377,7 +405,6 @@ class KeyboardTeleopNode(hm.HelloNode):
         rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
 
         rate = rospy.Rate(self.rate)
-
         while not rospy.is_shutdown():
             self.keys.print_commands()
             command = self.keys.get_command(self)

stretch_core/nodes/stretch_driver

@@ -146,7 +146,7 @@ class StretchBodyNode:
         for cg in self.joint_trajectory_action.command_groups:
             pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode,
                                manipulation_origin=self.mobile_base_manipulation_origin)
-            if cg.name == 'joint_gripper':
+            if cg.name == 'gripper_aperture':
                 joint_state.name.append('joint_gripper_finger_left')
                 joint_state.name.append('joint_gripper_finger_right')
                 joint_state.position.append(pos)