Merge pull request #66 from RethinkRobotics/core_msgs_sync

Core msgs sync

Author: Ian McMahon

src/baxter_interface/gripper.py

@@ -149,7 +149,7 @@ def _on_gripper_prop(self, properties):
             self.on_type_changed({
                 EndEffectorProperties.SUCTION_CUP_GRIPPER: 'suction',
                 EndEffectorProperties.ELECTRIC_GRIPPER: 'electric',
-                EndEffectorProperties.CUSTOM_GRIPPER: 'custom',
+                EndEffectorProperties.PASSIVE_GRIPPER: 'custom',
                                  }.get(properties.ui_type, None))
 
     def _inc_cmd_sequence(self):
@@ -380,7 +380,7 @@ def reset_custom_properties(self, timeout=2.0):
         @rtype: bool
         """
         default_id = 131073
-        default_ui_type = EndEffectorProperties.CUSTOM_GRIPPER
+        default_ui_type = EndEffectorProperties.PASSIVE_GRIPPER
         default_manufacturer = 'Rethink Research Robot'
         default_product = 'SDK End Effector'
         # Create default properties message
@@ -953,7 +953,7 @@ def type(self):
         return {
         EndEffectorProperties.SUCTION_CUP_GRIPPER: 'suction',
         EndEffectorProperties.ELECTRIC_GRIPPER: 'electric',
-        EndEffectorProperties.CUSTOM_GRIPPER: 'custom',
+        EndEffectorProperties.PASSIVE_GRIPPER: 'custom',
         }.get(self._prop.ui_type, None)
 
     def hardware_id(self):

src/baxter_interface/head.py

@@ -81,7 +81,7 @@ def __init__(self):
 
     def _on_head_state(self, msg):
         self._state['pan'] = msg.pan
-        self._state['panning'] = msg.isPanning
+        self._state['panning'] = msg.isTurning
         self._state['nodding'] = msg.isNodding
 
     def pan(self):
@@ -111,20 +111,32 @@ def panning(self):
         """
         return self._state['panning']
 
-    def set_pan(self, angle, speed=100, timeout=10.0):
+    def set_pan(self, angle, speed=1.0, timeout=10.0, scale_speed=False):
         """
         Pan at the given speed to the desired angle.
 
         @type angle: float
         @param angle: Desired pan angle in radians.
         @type speed: int
-        @param speed: Desired speed to pan at, range is 0-100 [100]
+        @param speed: Desired speed to pan at, range is 0-1.0 [1.0]
         @type timeout: float
         @param timeout: Seconds to wait for the head to pan to the
                         specified angle. If 0, just command once and
                         return. [10]
+        @param scale_speed: Scale speed to pan at by a factor of 100,
+                            to use legacy range between 0-100 [100]
         """
-        msg = HeadPanCommand(angle, speed)
+        if scale_speed:
+            cmd_speed = speed / 100.0;
+        else:
+            cmd_speed = speed
+        if (cmd_speed < HeadPanCommand.MIN_SPEED_RATIO or
+              cmd_speed > HeadPanCommand.MAX_SPEED_RATIO):
+            rospy.logerr(("Commanded Speed, ({0}), outside of valid range"
+                          " [{1}, {2}]").format(cmd_speed,
+                          HeadPanCommand.MIN_SPEED_RATIO,
+                          HeadPanCommand.MAX_SPEED_RATIO))
+        msg = HeadPanCommand(angle, cmd_speed, True)
         self._pub_pan.publish(msg)
 
         if not timeout == 0:

src/baxter_interface/navigator.py

@@ -30,13 +30,13 @@
 import baxter_dataflow
 
 from baxter_core_msgs.msg import (
-    ITBState,
+    NavigatorState,
 )
+
 from baxter_interface import (
     digital_io,
 )
 
-
 class Navigator(object):
     """
     Interface class for a Navigator on the Baxter robot.
@@ -82,17 +82,19 @@ def __init__(self, location):
         self.button2_changed = baxter_dataflow.Signal()
         self.wheel_changed = baxter_dataflow.Signal()
 
-        nav_state_topic = 'robot/itb/%s_itb/state' % (self._id,)
+        nav_state_topic = 'robot/navigators/{0}_navigator/state'.format(self._id)
         self._state_sub = rospy.Subscriber(
             nav_state_topic,
-            ITBState,
+            NavigatorState,
             self._on_state)
 
         self._inner_led = digital_io.DigitalIO(
-            '%s_itb_light_inner' % (self._id,))
+            '%s_inner_light' % (self._id,))
+        self._inner_led_idx = 0
 
         self._outer_led = digital_io.DigitalIO(
-            '%s_itb_light_outer' % (self._id,))
+            '%s_outer_light' % (self._id,))
+        self._outer_led_idx = 1
 
         init_err_msg = ("Navigator init failed to get current state from %s" %
                         (nav_state_topic,))
@@ -132,7 +134,7 @@ def inner_led(self):
         """
         Current state of the inner LED
         """
-        return self._state.innerLight
+        return self._state.lights[self._inner_led_idx]
 
     @inner_led.setter
     def inner_led(self, enable):
@@ -149,7 +151,7 @@ def outer_led(self):
         """
         Current state of the outer LED.
         """
-        return self._state.outerLight
+        return self._state.lights[self._outer_led_idx]
 
     @outer_led.setter
     def outer_led(self, enable):
@@ -164,7 +166,14 @@ def outer_led(self, enable):
     def _on_state(self, msg):
         if not self._state:
             self._state = msg
-
+            try:
+                self._inner_led_idx = self._state.light_names.index("inner")
+            except:
+                pass
+            try:
+                self._outer_led_idx = self._state.light_names.index("outer")
+            except:
+                pass
         if self._state == msg:
             return
 

src/baxter_interface/settings.py

@@ -29,13 +29,13 @@
 HEAD_PAN_ANGLE_TOLERANCE = 0.1396263401
 
 ## Versioning
-SDK_VERSION = '1.1.1'
+SDK_VERSION = '1.2.0'
 CHECK_VERSION = True
 # Version Compatibility Maps - {current: compatible}
-VERSIONS_SDK2ROBOT = {'1.1.1': ['1.1.1', '1.1.0']}
-VERSIONS_SDK2GRIPPER = {'1.1.1':
+VERSIONS_SDK2ROBOT = {'1.2.0': ['1.2.0']}
+VERSIONS_SDK2GRIPPER = {'1.2.0':
                           {
                            'warn': '2014/5/20 00:00:00',  # Version 1.0.0
                            'fail': '2013/10/15 00:00:00', # Version 0.6.2
                           }
-                         }
+                       }

src/head_action/head_action.py

@@ -41,6 +41,10 @@
 )
 
 import baxter_interface
+from baxter_core_msgs.msg import (
+   HeadPanCommand,
+)
+
 
 class HeadActionServer(object):
     def __init__(self, reconfig_server):
@@ -85,8 +89,8 @@ def _on_head_action(self, goal):
         position = goal.position
         velocity = goal.max_velocity
         # Apply max velocity if specified < 0
-        if velocity < 0.0:
-            velocity = 100.0
+        if velocity < HeadPanCommand.MIN_SPEED_RATIO:
+            velocity = HeadPanCommand.MAX_SPEED_RATIO
 
         # Pull parameters that will define the head actuation
         self._get_head_parameters()