Make NavigateThroughPoses navigator report waypoint statuses information

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

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" waypoint_statuses_id="{waypoint_statuses_id}" />
               <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

@@ -23,6 +23,7 @@
 #include "geometry_msgs/msg/pose_stamped.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"
@@ -109,6 +110,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;
 
@@ -100,6 +107,9 @@ NavigateThroughPosesNavigator::goalCompleted(
       result->error_code,
       result->error_msg.c_str());
   }
+  auto blackboard = bt_action_server_->getBlackboard();
+  result->waypoint_statuses =
+    blackboard->get<std::vector<nav2_msgs::msg::WaypointStatus>>(waypoint_statuses_blackboard_id_);
 }
 
 void
@@ -116,6 +126,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 +275,16 @@ 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
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(goals_array.goals.size());
+  for (size_t waypoint_index = 0 ; waypoint_index < goals_array.goals.size() ; ++waypoint_index) {
+    waypoint_statuses[waypoint_index].index = waypoint_index;
+    waypoint_statuses[waypoint_index].goal = goals_array.goals[waypoint_index];
+  }
+  blackboard->set<decltype(waypoint_statuses)>(waypoint_statuses_blackboard_id_,
+      std::move(waypoint_statuses));
+  blackboard->set<std::string>("waypoint_statuses_id", waypoint_statuses_blackboard_id_);
+
   return true;
 }