From bebf7c4b0974b27416dd15e064b41391b087d241 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Tue, 23 Jun 2020 19:17:01 -0400 Subject: [PATCH] Fixed misfail in set_goal of MobileBaseCommandGroup --- stretch_core/nodes/command_groups.py | 6 ++++-- stretch_core/nodes/joint_trajectory_server.py | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index ce52be2..1ccbbec 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -461,11 +461,13 @@ class MobileBaseCommandGroup: if self.use_mobile_base_inc_rot: self.goal_rotate_mobile_base_mecha = point.positions[self.rotate_mobile_base_index] - self.rotate_mobile_base_goal = True + if not np.isclose(self.goal_rotate_mobile_base_mecha, 0.0, rtol=1e-5, atol=1e-8, equal_nan=False): + self.rotate_mobile_base_goal = True if self.use_mobile_base_inc_trans: self.goal_translate_mobile_base_mecha = point.positions[self.translate_mobile_base_index] - self.translate_mobile_base_goal = True + if not np.isclose(self.goal_translate_mobile_base_mecha, 0.0, rtol=1e-5, atol=1e-8, equal_nan=False): + self.translate_mobile_base_goal = True if self.rotate_mobile_base_goal and self.translate_mobile_base_goal: err_str = 'Received a goal point with simultaneous rotation and translation mobile base goals. \ diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index ab5292e..923ece6 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -160,7 +160,7 @@ class JointTrajectoryAction: 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.result.set_aborted(self.result) + 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))