Add waypoint_statuses records in RemoveInCollisionGoals/RemovePassedGoals actions

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

Author: zz990099

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

@@ -23,6 +23,7 @@
 #include "nav_msgs/msg/goals.hpp"
 #include "nav2_behavior_tree/bt_service_node.hpp"
 #include "nav2_msgs/srv/get_costs.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -63,6 +64,10 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
           "Whether to consider unknown cost as obstacle"),
         BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
           "Goals with in-collision goals removed"),
+        BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
+          "Original waypoint statuses vector to be modified"),
+        BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
+          "waypoint statuses vector with states modified"),
       });
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

@@ -24,6 +24,7 @@
 #include "nav2_util/robot_utils.hpp"
 #include "behaviortree_cpp/action_node.h"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -49,6 +50,10 @@ class RemovePassedGoals : public BT::ActionNodeBase
           "Goals with passed viapoints removed"),
       BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
       BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),
+      BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
+          "Original waypoint statuses vector to be modified"),
+      BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
+          "waypoint statuses vector with states modified"),
     };
   }
 

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -63,12 +63,18 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
     return BT::NodeStatus::FAILURE;
   }
 
+  // get the `waypoint_statuses` vector if provided
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
+  auto get_waypoint_statuses_res = getInput("input_waypoint_statuses", waypoint_statuses);
+
   nav_msgs::msg::Goals valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
     if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
       response->costs[i] < cost_threshold_)
     {
       valid_goal_poses.goals.push_back(input_goals_.goals[i]);
+    } else if (get_waypoint_statuses_res) {
+      waypoint_statuses[i].waypoint_status = nav2_msgs::msg::WaypointStatus::SKIPPED;
     }
   }
   // Inform if all goals have been removed
@@ -78,6 +84,9 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
       "All goals are in collision and have been removed from the list");
   }
   setOutput("output_goals", valid_goal_poses);
+  if (get_waypoint_statuses_res) {
+    setOutput("output_waypoint_statuses", waypoint_statuses);
+  }
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

@@ -67,6 +67,21 @@ inline BT::NodeStatus RemovePassedGoals::tick()
     return BT::NodeStatus::FAILURE;
   }
 
+  // get the `waypoint_statuses` vector if provided
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
+  auto get_waypoint_statuses_res = getInput("input_waypoint_statuses", waypoint_statuses);
+  size_t cur_waypoint_index = 0;
+  if (get_waypoint_statuses_res) {
+    while (cur_waypoint_index < waypoint_statuses.size()) {
+      if (waypoint_statuses[cur_waypoint_index].waypoint_status ==
+        nav2_msgs::msg::WaypointStatus::PENDING)
+      {
+        break;
+      }
+      ++cur_waypoint_index;
+    }
+  }
+
   double dist_to_goal;
   while (goal_poses.goals.size() > 1) {
     dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose);
@@ -76,9 +91,18 @@ inline BT::NodeStatus RemovePassedGoals::tick()
     }
 
     goal_poses.goals.erase(goal_poses.goals.begin());
+    // mark waypoint statuses if the vector is provided
+    if (get_waypoint_statuses_res) {
+      waypoint_statuses[cur_waypoint_index].waypoint_status =
+        nav2_msgs::msg::WaypointStatus::COMPLETED;
+      ++cur_waypoint_index;
+    }
   }
 
   setOutput("output_goals", goal_poses);
+  if (get_waypoint_statuses_res) {
+    setOutput("output_waypoint_statuses", waypoint_statuses);
+  }
 
   return BT::NodeStatus::SUCCESS;
 }