Make WaypointFollower use WaypointStatus message type in action result

Signed-off-by: zz990099 <[email protected]>

Author: zz990099

nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp

@@ -27,7 +27,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_msgs/action/follow_waypoints.hpp"
-#include "nav2_msgs/msg/missed_waypoint.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/node_utils.hpp"

nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -305,9 +305,10 @@ void WaypointFollower::followWaypointsHandler(
       current_goal_status_.status == ActionStatus::FAILED ||
       current_goal_status_.status == ActionStatus::UNKNOWN)
     {
-      nav2_msgs::msg::MissedWaypoint missedWaypoint;
-      missedWaypoint.index = goal_index;
-      missedWaypoint.goal = poses[goal_index];
+      nav2_msgs::msg::WaypointStatus missedWaypoint;
+      missedWaypoint.waypoint_status = nav2_msgs::msg::WaypointStatus::FAILED;
+      missedWaypoint.waypoint_index = goal_index;
+      missedWaypoint.waypoint_pose = 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);
@@ -340,9 +341,10 @@ void WaypointFollower::followWaypointsHandler(
         is_task_executed ? "succeeded" : "failed!");
 
       if (!is_task_executed) {
-        nav2_msgs::msg::MissedWaypoint missedWaypoint;
-        missedWaypoint.index = goal_index;
-        missedWaypoint.goal = poses[goal_index];
+        nav2_msgs::msg::WaypointStatus missedWaypoint;
+        missedWaypoint.waypoint_status = nav2_msgs::msg::WaypointStatus::FAILED;
+        missedWaypoint.waypoint_index = goal_index;
+        missedWaypoint.waypoint_pose = poses[goal_index];
         missedWaypoint.error_code =
           nav2_msgs::action::FollowWaypoints::Result::TASK_EXECUTOR_FAILED;
         missedWaypoint.error_msg = "Task execution failed";