From f4def9111cd4aeb6ad83c910116fb7a49acd5669 Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Wed, 24 Jun 2020 01:11:13 -0400
Subject: [PATCH] Fixed multiple aborts error

---
 stretch_core/nodes/joint_trajectory_server.py | 27 ++++++++++---------
 1 file changed, 15 insertions(+), 12 deletions(-)

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 = []