Fix waypoint_follower error_msg handling (#4341)

Include error_msg in reason for missed waypoint Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Aug 28, 2024 · 6cbed01 · 6cbed01
1 parent 9c9f22f
commit 6cbed01
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git 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
@@ -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<ClientT>::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());
   }
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -56,6 +56,7 @@ struct GoalStatus @@
     {
       ActionStatus status;
       int error_code;
+      std::string error_msg;
     };
     /**
@@ Expand Down @@