From 72ade5d11ea0f1a37e6a2f6c47f867aca3aa066b 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 958e72ec5..1a709fc45 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 @@ -406,6 +406,8 @@ update(const ros::Time& time, const ros::Duration& period) rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; 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(); } @@ -441,6 +443,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->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(); }