Fix joint trajectory controller so results message is returned on tol…

…erance failures
ros-controls · Mar 29, 2022 · 95ebe05 · 95ebe05
1 parent 9af3234
commit 95ebe05
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/...jectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/...jectory_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();
         }