Merge remote-tracking branch 'origin/release-1.2.0'

Author: IanTheEngineer

CHANGELOG.rst

@@ -1,3 +1,16 @@
+1.2.0 (2015-12-21)
+---------------------------------
+- Added an optional parameter to Limb interface's move_to_joint_positions() to allow it to be aborted by test function
+  Thanks to Yoan Mollard (ymollard) for submitting this pull request!
+- Added a wait for the endpoint_state messages to be received before exiting Limb init
+- Fixed a bug in the JTAS that would cause the robot to jump back into the last commanded pose when the 
+  robot is Disabled/Re-enabled
+- Fixed a bug that would cause the Limb's on_joint_states method to fail if extra <side> joints are added to the 
+  robot's /robot/joint_states
+- Due to baxter_core_msgs change, updated EndEffectorProperties CUSTOM_GRIPPER to PASSIVE_GRIPPER
+- Due to baxter_core_msgs change, updated head interface's speed range from [0, 100] to [0, 1.0]
+- Due to baxter_core_msgs change, updated Navigator interface to use update Nav topics and lights msg field
+
 1.1.1 (2015-5-15)
 ---------------------------------
 -  Fixed a bug that caused the JTAS to error with a path of one or two points is supplied as a trajectory

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>baxter_interface</name>
-  <version>1.1.1</version>
+  <version>1.2.0</version>
   <description>
     Convenient python interface classes for control
     of the Baxter Research Robot from Rethink Robotics.

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/limb.py

@@ -119,10 +119,14 @@ def __init__(self, limb):
                    "from %s") % (self.name.capitalize(), joint_state_topic)
         baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
                                  timeout_msg=err_msg)
+        err_msg = ("%s limb init failed to get current endpoint_state "
+                   "from %s") % (self.name.capitalize(), ns + 'endpoint_state')
+        baxter_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
+                                 timeout_msg=err_msg)
 
     def _on_joint_states(self, msg):
         for idx, name in enumerate(msg.name):
-            if self.name in name:
+            if name in self._joint_names[self.name]:
                 self._joint_angle[name] = msg.position[idx]
                 self._joint_velocity[name] = msg.velocity[idx]
                 self._joint_effort[name] = msg.effort[idx]
@@ -399,7 +403,8 @@ def move_to_neutral(self, timeout=15.0):
         return self.move_to_joint_positions(angles, timeout)
 
     def move_to_joint_positions(self, positions, timeout=15.0,
-                                threshold=settings.JOINT_ANGLE_TOLERANCE):
+                                threshold=settings.JOINT_ANGLE_TOLERANCE,
+                                test=None):
         """
         (Blocking) Commands the limb to the provided positions.
 
@@ -414,6 +419,7 @@ def move_to_joint_positions(self, positions, timeout=15.0,
         @type threshold: float
         @param threshold: position threshold in radians across each joint when
         move is considered successful [0.008726646]
+        @param test: optional function returning True if motion must be aborted
         """
         cmd = self.joint_angles()
 
@@ -433,7 +439,8 @@ def joint_diff():
 
         self.set_joint_positions(filtered_cmd())
         baxter_dataflow.wait_for(
-            lambda: (all(diff() < threshold for diff in diffs)),
+            test=lambda: callable(test) and test() == True or \
+                         (all(diff() < threshold for diff in diffs)),
             timeout=timeout,
             timeout_msg=("%s limb failed to reach commanded joint positions" %
                          (self.name.capitalize(),)),

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()

src/joint_trajectory_action/joint_trajectory_action.py

@@ -70,6 +70,7 @@ def __init__(self, limb, reconfig_server, rate=100.0,
             auto_start=False)
         self._action_name = rospy.get_name()
         self._limb = baxter_interface.Limb(limb)
+        self._enable = baxter_interface.RobotEnable()
         self._name = limb
         self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb,))
         self._cuff.state_changed.connect(self._cuff_cb)
@@ -120,6 +121,9 @@ def __init__(self, limb, reconfig_server, rate=100.0,
             tcp_nodelay=True,
             queue_size=1)
 
+    def robot_is_enabled(self):
+        return self._enable.state().enabled
+
     def clean_shutdown(self):
         self._alive = False
         self._limb.exit_control_mode()
@@ -225,7 +229,8 @@ def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
         if self._mode == 'velocity':
             velocities = [0.0] * len(joint_names)
             cmd = dict(zip(joint_names, velocities))
-            while (not self._server.is_new_goal_available() and self._alive):
+            while (not self._server.is_new_goal_available() and self._alive
+                   and self.robot_is_enabled()):
                 self._limb.set_joint_velocities(cmd)
                 if self._cuff_state:
                     self._limb.exit_control_mode()
@@ -240,7 +245,8 @@ def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
                     pnt.velocities = [0.0] * len(joint_names)
                 if dimensions_dict['accelerations']:
                     pnt.accelerations = [0.0] * len(joint_names)
-            while (not self._server.is_new_goal_available() and self._alive):
+            while (not self._server.is_new_goal_available() and self._alive
+                   and self.robot_is_enabled()):
                 self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
                 # zero inverse dynamics feedforward command
                 if self._mode == 'position_w_id':
@@ -253,16 +259,16 @@ def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
                 rospy.sleep(1.0 / self._control_rate)
 
     def _command_joints(self, joint_names, point, start_time, dimensions_dict):
-        if self._server.is_preempt_requested():
+        if self._server.is_preempt_requested() or not self.robot_is_enabled():
             rospy.loginfo("%s: Trajectory Preempted" % (self._action_name,))
             self._server.set_preempted()
             self._command_stop(joint_names, self._limb.joint_angles(), start_time, dimensions_dict)
             return False
         velocities = []
         deltas = self._get_current_error(joint_names, point.positions)
         for delta in deltas:
-            if (math.fabs(delta[1]) >= self._path_thresh[delta[0]]
-                and self._path_thresh[delta[0]] >= 0.0):
+            if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]]
+                and self._path_thresh[delta[0]] >= 0.0)) or not self.robot_is_enabled():
                 rospy.logerr("%s: Exceeded Error Threshold on %s: %s" %
                              (self._action_name, delta[0], str(delta[1]),))
                 self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED
@@ -406,7 +412,8 @@ def _on_trajectory_action(self, goal):
         # Keep track of current indices for spline segment generation
         now_from_start = rospy.get_time() - start_time
         end_time = trajectory_points[-1].time_from_start.to_sec()
-        while (now_from_start < end_time and not rospy.is_shutdown()):
+        while (now_from_start < end_time and not rospy.is_shutdown() and
+               self.robot_is_enabled()):
             #Acquire Mutex
             now = rospy.get_time()
             now_from_start = now - start_time
@@ -451,7 +458,7 @@ def check_goal_state():
                 return True
 
         while (now_from_start < (last_time + self._goal_time)
-               and not rospy.is_shutdown()):
+               and not rospy.is_shutdown() and self.robot_is_enabled()):
             if not self._command_joints(joint_names, last, start_time, dimensions_dict):
                 return
             now_from_start = rospy.get_time() - start_time