From 45963530232887f4fa2c97713b546012a11ca6e5 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Thu, 2 Jul 2020 00:06:46 -0400 Subject: [PATCH] Contact triggers action server success callback --- stretch_core/nodes/command_groups.py | 24 +++++++++++++++++++ stretch_core/nodes/joint_trajectory_server.py | 15 ++++++++---- 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index 437d42c..eb8d14b 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -427,8 +427,12 @@ class TelescopingCommandGroup(SimpleCommandGroup): def update_execution(self, robot_status, **kwargs): backlash_state = kwargs['backlash_state'] + success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None self.error = None if self.active: + if success_callback and robot_status['arm']['motor']['in_guarded_event']: + success_callback("{0} contact detected.".format(self.name)) + return True if backlash_state['wrist_extension_retracted']: arm_backlash_correction = self.wrist_extension_calibrated_retracted_offset else: @@ -454,8 +458,12 @@ class LiftCommandGroup(SimpleCommandGroup): if self.goal['contact_threshold'] is not None else None) def update_execution(self, robot_status, **kwargs): + success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None self.error = None if self.active: + if success_callback and robot_status['lift']['motor']['in_guarded_event']: + success_callback("{0} contact detected.".format(self.name)) + return True self.error = self.goal['position'] - robot_status['lift']['pos'] return self.name, self.error @@ -616,6 +624,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup): contact_thresh_N=self.goal['contact_threshold']) def update_execution(self, robot_status, **kwargs): + success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None currx = robot_status['base']['x'] curry = robot_status['base']['y'] currtheta = robot_status['base']['theta'] @@ -627,14 +636,29 @@ class MobileBaseCommandGroup(SimpleCommandGroup): if self.active: if self.active_translate_mobile_base or self.active_rotate_mobile_base: if self.goal_translate_mobile_base['position'] is not None: + if (robot_status['base']['left_wheel']['in_guarded_event'] or \ + robot_status['base']['right_wheel']['in_guarded_event']) and \ + success_callback: + success_callback("translate_mobile_base contact detected.") + return True dist = np.sqrt(np.square(currx - self.startx) + np.square(curry - self.starty)) self.error_translate_mobile_base_m = self.goal_translate_mobile_base['position'] - (dist * np.sign(self.goal_translate_mobile_base['position'])) return [('translate_mobile_base', self.error_translate_mobile_base_m), ('rotate_mobile_base', self.error_rotate_mobile_base_rad)] elif self.goal_rotate_mobile_base['position'] is not None: + if (robot_status['base']['left_wheel']['in_guarded_event'] or \ + robot_status['base']['right_wheel']['in_guarded_event']) and \ + success_callback: + success_callback("rotate_mobile_base contact detected.") + return True rot = hm.angle_diff_rad(currtheta, self.starttheta) self.error_rotate_mobile_base_rad = hm.angle_diff_rad(self.goal_rotate_mobile_base['position'], rot) return [('translate_mobile_base', self.error_translate_mobile_base_m), ('rotate_mobile_base', self.error_rotate_mobile_base_rad)] else: + if (robot_status['base']['left_wheel']['in_guarded_event'] or \ + robot_status['base']['right_wheel']['in_guarded_event']) and \ + success_callback: + success_callback("{0} contact detected.".format(self.name)) + return True self.error = self.goal['position'] - currx return self.name, self.error diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 6aaef62..2945af2 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -140,7 +140,12 @@ class JointTrajectoryAction: return robot_status = self.node.robot.get_status() - named_errors = [c.update_execution(robot_status, backlash_state=self.node.backlash_state) for c in command_groups] + named_errors = [c.update_execution(robot_status, success_callback=self.success_callback, + backlash_state=self.node.backlash_state) + for c in command_groups] + if any(ret == True for ret in named_errors): + self.node.robot_mode_rwlock.release_read() + return self.feedback_callback(commanded_joint_names, point, named_errors) goals_reached = [c.goal_reached() for c in command_groups] @@ -148,7 +153,7 @@ class JointTrajectoryAction: rospy.logdebug("{0} joint_traj action: Achieved target point.".format(self.node.node_name)) - self.success_callback() + self.success_callback("Achieved all target points.") self.node.robot_mode_rwlock.release_read() return @@ -196,8 +201,8 @@ class JointTrajectoryAction: self.feedback.error = error_point self.server.publish_feedback(self.feedback) - def success_callback(self): - rospy.loginfo("{0} joint_traj action: Achieved all target points!".format(self.node.node_name)) + def success_callback(self, success_str): + rospy.loginfo("{0} joint_traj action: {1}".format(self.node.node_name, success_str)) self.result.error_code = self.result.SUCCESSFUL - self.result.error_string = "Achieved all target points!" + self.result.error_string = success_str self.server.set_succeeded(self.result)