FollowWaypoints - Add usage of error_code and error_msg (ros-navigati…

…on#4341)

Introduces error codes:-
- NO_WAYPOINTS_GIVEN
- STOP_ON_MISSED_WAYPOINT

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>

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

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;