diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 258c456..f3f8606 100644 --- a/CHANGELOG.rst +++ b/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 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 diff --git a/package.xml b/package.xml index bc5d9fa..8944f13 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ baxter_interface - 1.1.1 + 1.2.0 Convenient python interface classes for control of the Baxter Research Robot from Rethink Robotics. diff --git a/src/baxter_interface/limb.py b/src/baxter_interface/limb.py index 0834a6e..0201c3d 100644 --- a/src/baxter_interface/limb.py +++ b/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] diff --git a/src/joint_trajectory_action/joint_trajectory_action.py b/src/joint_trajectory_action/joint_trajectory_action.py index 04f101e..99b2171 100644 --- a/src/joint_trajectory_action/joint_trajectory_action.py +++ b/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,7 +259,7 @@ 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) @@ -261,8 +267,8 @@ def _command_joints(self, joint_names, point, start_time, dimensions_dict): 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