diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 8dd0d70cd77..1ec60bebc73 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -56,6 +56,7 @@ struct GoalStatus { ActionStatus status; int error_code; + std::string error_msg; }; /** diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 436520d09b0..e1586ccb15c 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -308,6 +308,7 @@ void WaypointFollower::followWaypointsHandler( missedWaypoint.index = goal_index; missedWaypoint.goal = poses[goal_index]; missedWaypoint.error_code = current_goal_status_.error_code; + missedWaypoint.error_msg = current_goal_status_.error_msg; result->missed_waypoints.push_back(missedWaypoint); if (stop_on_failure_) { @@ -431,13 +432,16 @@ WaypointFollower::resultCallback( case rclcpp_action::ResultCode::ABORTED: current_goal_status_.status = ActionStatus::FAILED; current_goal_status_.error_code = result.result->error_code; + current_goal_status_.error_msg = result.result->error_msg; return; case rclcpp_action::ResultCode::CANCELED: current_goal_status_.status = ActionStatus::FAILED; return; default: - RCLCPP_ERROR(get_logger(), "Received an UNKNOWN result code from navigation action!"); current_goal_status_.status = ActionStatus::UNKNOWN; + current_goal_status_.error_code = nav2_msgs::action::FollowWaypoints::Result::UNKNOWN; + current_goal_status_.error_msg = "Received an UNKNOWN result code from navigation action!"; + RCLCPP_ERROR(get_logger(), current_goal_status_.error_msg.c_str()); return; } } @@ -447,10 +451,10 @@ WaypointFollower::goalResponseCallback( const rclcpp_action::ClientGoalHandle::SharedPtr & goal) { if (!goal) { - RCLCPP_ERROR( - get_logger(), - "navigate_to_pose action client failed to send goal to server."); current_goal_status_.status = ActionStatus::FAILED; + current_goal_status_.error_code = nav2_msgs::action::FollowWaypoints::Result::UNKNOWN; + current_goal_status_.error_msg = "navigate_to_pose action client failed to send goal to server."; + RCLCPP_ERROR(get_logger(), current_goal_status_.error_msg.c_str()); } }