diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0aecc64184..7d66a2c3c1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -343,6 +343,7 @@ controller_interface::return_type JointTrajectoryController::update( { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + result->set__error_string("Aborted due to path tolerance violation"); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 @@ -359,9 +360,10 @@ controller_interface::return_type JointTrajectoryController::update( { if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + result->set__error_string("Goal successfully reached!"); + active_goal->setSucceeded(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); @@ -374,17 +376,19 @@ controller_interface::return_type JointTrajectoryController::update( } else if (!within_goal_time) { + const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " + + std::to_string(time_difference) + " seconds"; + auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + result->set__error_string(error_string); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position());