Skip to content

Commit

Permalink
[JTC] Fill action error_strings (#887)
Browse files Browse the repository at this point in the history
(cherry picked from commit 67dbf7b)
  • Loading branch information
christophfroehlich authored and mergify[bot] committed Jan 31, 2024
1 parent b0c1330 commit 768f3f1
Showing 1 changed file with 10 additions and 6 deletions.
16 changes: 10 additions & 6 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,7 @@ controller_interface::return_type JointTrajectoryController::update(
{
auto result = std::make_shared<FollowJTrajAction::Result>();
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
Expand All @@ -362,9 +363,10 @@ controller_interface::return_type JointTrajectoryController::update(
{
if (!outside_goal_tolerance)
{
auto res = std::make_shared<FollowJTrajAction::Result>();
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
active_goal->setSucceeded(res);
auto result = std::make_shared<FollowJTrajAction::Result>();
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());
Expand All @@ -377,17 +379,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<FollowJTrajAction::Result>();
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());
Expand Down

0 comments on commit 768f3f1

Please sign in to comment.