Merge pull request #71 from RethinkRobotics/release-1.2.0

Release 1.2.0

Author: Ian McMahon

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

@@ -126,7 +126,7 @@ def __init__(self, limb):
 
     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]

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