From 95ebe05483f4b2381b343bb76cdec6135b044007 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 28 Mar 2022 10:12:23 -0500 Subject: [PATCH] Fix joint trajectory controller so results message is returned on tolerance failures --- .../joint_trajectory_controller_impl.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 9a39cd5c0..3e63ff06f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -407,6 +407,8 @@ update(const ros::Time& time, const ros::Duration& period) control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; rt_segment_goal->preallocated_result_->error_string = joint_names_[i] + " path error " + std::to_string( state_joint_error_.position[0] ); rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + // Force this to run before destroying rt_active_goal_ so results message is returned + rt_active_goal_->runNonRealtime(ros::TimerEvent()); rt_active_goal_.reset(); successful_joint_traj_.reset(); } @@ -443,6 +445,8 @@ update(const ros::Time& time, const ros::Duration& period) rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; rt_segment_goal->preallocated_result_->error_string = joint_names_[i] + " goal error " + std::to_string( state_joint_error_.position[0] ); rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + // Force this to run before destroying rt_active_goal_ so results message is returned + rt_active_goal_->runNonRealtime(ros::TimerEvent()); rt_active_goal_.reset(); successful_joint_traj_.reset(); }