Fix joint trajectory controller so results message is returned on tolerance failures

Author: Levi-Armstrong

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