Re #65 Prevent JTAS execution when Re-Enabled

This fix ensures that the Trajectory callback
of the JTAS exits whenever the robot has been
disabled. This results in the robot holding
its current pose when it is re-enabled, rather
than attempting to return to the originally
commanded location.

Author: IanTheEngineer

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