FollowWaypoints - Add usage of error_code and error_msg (#4341)

Introduces error codes:- - NO_WAYPOINTS_GIVEN - STOP_ON_MISSED_WAYPOINT Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Jun 22, 2024 · da5e15a · da5e15a
1 parent c03a783
commit da5e15a
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 11 deletions.
diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action
@@ -10,6 +10,8 @@ geographic_msgs/GeoPose[] gps_poses
 uint16 NONE=0
 uint16 UNKNOWN=600
 uint16 TASK_EXECUTOR_FAILED=601
+uint16 NO_WAYPOINTS_GIVEN=602
+uint16 STOP_ON_MISSED_WAYPOINT=603
 
 MissedWaypoint[] missed_waypoints
 int16 error_code

diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action
@@ -10,6 +10,8 @@ geometry_msgs/PoseStamped[] poses
 uint16 NONE=0
 uint16 UNKNOWN=600
 uint16 TASK_EXECUTOR_FAILED=601
+uint16 NO_WAYPOINTS_GIVEN=602
+uint16 STOP_ON_MISSED_WAYPOINT=603
 
 MissedWaypoint[] missed_waypoints
 uint16 error_code

diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -219,7 +219,7 @@ class PlannerServer : public nav2_util::LifecycleNode
     const geometry_msgs::msg::PoseStamped & goal,
     const std::string & planner_id,
     const std::exception & ex,
-    std::string& msg);
+    std::string & msg);
 
   /**
    * @brief Callback executed when a parameter change is detected

diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
@@ -738,7 +738,7 @@ void PlannerServer::exceptionWarning(
   const geometry_msgs::msg::PoseStamped & goal,
   const std::string & planner_id,
   const std::exception & ex,
-  std::string& error_msg)
+  std::string & error_msg)
 {
   error_msg = planner_id +
     "plugin failed to plan from (" +

diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -230,6 +230,8 @@ void WaypointFollower::followWaypointsHandler(
     static_cast<int>(poses.size()));
 
   if (poses.empty()) {
+    result->error_code =
+      nav2_msgs::action::FollowWaypoints::Result::NO_WAYPOINTS_GIVEN;
     result->error_msg =
       "Empty vector of waypoints passed to waypoint following "
       "action potentially due to conversation failure or empty request.";
@@ -261,6 +263,8 @@ void WaypointFollower::followWaypointsHandler(
       goal = action_server->accept_pending_goal();
       poses = getLatestGoalPoses<T>(action_server);
       if (poses.empty()) {
+        result->error_code =
+          nav2_msgs::action::FollowWaypoints::Result::NO_WAYPOINTS_GIVEN;
         result->error_msg =
           "Empty vector of Waypoints passed to waypoint following logic. "
           "Nothing to execute, returning with failure!";
@@ -306,10 +310,13 @@ void WaypointFollower::followWaypointsHandler(
       result->missed_waypoints.push_back(missedWaypoint);
 
       if (stop_on_failure_) {
-        RCLCPP_WARN(
-          get_logger(), "Failed to process waypoint %i in waypoint "
-          "list and stop on failure is enabled."
-          " Terminating action.", goal_index);
+        result->error_code =
+          nav2_msgs::action::FollowWaypoints::Result::STOP_ON_MISSED_WAYPOINT;
+        result->error_msg =
+          "Failed to process waypoint " + std::to_string(goal_index) +
+          " in waypoint list and stop on failure is enabled."
+          " Terminating action.";
+        RCLCPP_WARN(get_logger(), result->error_msg.c_str());
         action_server->terminate_current(result);
         current_goal_status_.error_code = 0;
         return;
@@ -338,11 +345,12 @@ void WaypointFollower::followWaypointsHandler(
       }
       // if task execution was failed and stop_on_failure_ is on , terminate action
       if (!is_task_executed && stop_on_failure_) {
-        RCLCPP_WARN(
-          get_logger(), "Failed to execute task at waypoint %i "
-          " stop on failure is enabled."
-          " Terminating action.", goal_index);
-
+        result->error_code =
+          nav2_msgs::action::FollowWaypoints::Result::TASK_EXECUTOR_FAILED;
+        result->error_msg =
+          "Failed to execute task at waypoint " + std::to_string(goal_index) +
+          " stop on failure is enabled. Terminating action.";
+        RCLCPP_WARN(get_logger(), result->error_msg.c_str());
         action_server->terminate_current(result);
         current_goal_status_.error_code = 0;
         return;