diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index a09824a..6b0342a 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -142,22 +142,25 @@ class JointTrajectoryAction: return def invalid_joints_callback(self, err_str): - rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) - self.result.error_code = self.result.INVALID_JOINTS - self.result.error_string = err_str - self.server.set_aborted(self.result) + if self.server.is_active() or self.server.is_preempt_requested(): + rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) + self.result.error_code = self.result.INVALID_JOINTS + self.result.error_string = err_str + self.server.set_aborted(self.result) def invalid_goal_callback(self, err_str): - rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) - self.result.error_code = self.result.INVALID_GOAL - self.result.error_string = err_str - self.server.set_aborted(self.result) + if self.server.is_active() or self.server.is_preempt_requested(): + rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) + self.result.error_code = self.result.INVALID_GOAL + self.result.error_string = err_str + self.server.set_aborted(self.result) def goal_tolerance_violated_callback(self, err_str): - rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) - self.result.error_code = self.result.GOAL_TOLERANCE_VIOLATED - self.result.error_string = err_str - self.server.set_aborted(self.result) + if self.server.is_active() or self.server.is_preempt_requested(): + rospy.logerr('{0} joint_traj action: {1}'.format(self.node.node_name, err_str)) + self.result.error_code = self.result.GOAL_TOLERANCE_VIOLATED + self.result.error_string = err_str + self.server.set_aborted(self.result) def feedback_callback(self, commanded_joint_names, desired_point, named_errors): clean_named_errors = []