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(); }