From a67be9981d4c99d7cf90375a20d253ea0c18e08d Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Fri, 19 Jun 2020 02:36:51 -0400 Subject: [PATCH] Refactored execution loop condition --- stretch_core/nodes/joint_trajectory_server.py | 24 +++++++------------ 1 file changed, 9 insertions(+), 15 deletions(-) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 306c10b..1178eb8 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -98,13 +98,20 @@ class JointTrajectoryAction: robot_status = self.node.robot.get_status() # uses lock held by robot [c.init_execution(robot_status) for c in command_groups] + goals_reached = [c.goal_reached() for c in command_groups] incremental_commands_executed = False update_rate = rospy.Rate(15.0) goal_start_time = rospy.Time.now() - while True: - robot_status = self.node.robot.get_status() + while not all(goals_reached): + if (rospy.Time.now() - goal_start_time) > self.node.default_goal_timeout_duration: + err_str = 'Time to execute the current goal point = <{0}> exceeded the \ + default_goal_timeout = {1}'.format(point, self.node.default_goal_timeout_s) + self.goal_tolerance_violated_callback(err_str) + self.node.robot_mode_rwlock.release_read() + return + robot_status = self.node.robot.get_status() if self.node.use_lift: lift_error_m = self.lift_cg.update_execution(robot_status, self.node.backlash_state) extension_error_m = self.telescoping_cg.update_execution(robot_status, @@ -178,20 +185,7 @@ class JointTrajectoryAction: self.node.robot.push_command() incremental_commands_executed = True - # Check if the goal positions have been reached. goals_reached = [c.goal_reached() for c in command_groups] - if all(goals_reached): - if self.node.trajectory_debug: - rospy.loginfo('achieved goal!') - break - - if (rospy.Time.now() - goal_start_time) > self.node.default_goal_timeout_duration: - err_str = 'Time to execute the current goal point = <{0}> exceeded the \ - default_goal_timeout = {1}'.format(point, self.node.default_goal_timeout_s) - self.goal_tolerance_violated_callback(err_str) - self.node.robot_mode_rwlock.release_read() - return - update_rate.sleep() rospy.logdebug('{0} joint_traj action: Achieved target point.'.format(self.node.node_name))