Make NavigateThroughPoses navigator report waypoint statuses information

Signed-off-by: zz990099 <771647586@qq.com>

Author: zz990099

nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml

@@ -14,7 +14,7 @@
         <RateController hz="0.333">
           <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
             <ReactiveSequence>
-              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
               <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
             </ReactiveSequence>
             <Sequence>

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp

@@ -24,6 +24,7 @@
 #include "geometry_msgs/msg/pose_stamped_array.hpp"
 #include "nav2_core/behavior_tree_navigator.hpp"
 #include "nav2_msgs/action/navigate_through_poses.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -110,6 +111,7 @@ class NavigateThroughPosesNavigator
   rclcpp::Time start_time_;
   std::string goals_blackboard_id_;
   std::string path_blackboard_id_;
+  std::string waypoint_statuses_blackboard_id_;
 
   // Odometry smoother object
   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -42,6 +42,13 @@ NavigateThroughPosesNavigator::configure(
 
   path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
 
+  if (!node->has_parameter("waypoint_statuses_blackboard_id")) {
+    node->declare_parameter("waypoint_statuses_blackboard_id", std::string("waypoint_statuses"));
+  }
+
+  waypoint_statuses_blackboard_id_ =
+    node->get_parameter("waypoint_statuses_blackboard_id").as_string();
+
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;
 
@@ -116,6 +123,9 @@ NavigateThroughPosesNavigator::onLoop()
   nav_msgs::msg::Goals goal_poses;
   [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses);
 
+  feedback_msg->waypoint_statuses =
+    blackboard->get<std::vector<nav2_msgs::msg::WaypointStatus>>(waypoint_statuses_blackboard_id_);
+
   if (goal_poses.goals.size() == 0) {
     bt_action_server_->publishFeedback(feedback_msg);
     return;
@@ -262,6 +272,11 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
   blackboard->set<nav_msgs::msg::Goals>(goals_blackboard_id_,
       std::move(goals_array));
 
+  // Reset the waypoint states vector in the blackboard
+  auto waypoint_statuses = std::vector<nav2_msgs::msg::WaypointStatus>(goals_array.goals.size());
+  blackboard->set<decltype(waypoint_statuses)>(waypoint_statuses_blackboard_id_,
+      std::move(waypoint_statuses));
+
   return true;
 }