Make NavigateThroughPoses navigator report waypoint statuses information. (fix #4846)

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 to mark waypoint status from"),
+        BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
+          "Waypoint_statuses with in-collision waypoints marked")
       });
   }
 

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 to mark waypoint status from"),
+      BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
+          "Waypoint_statuses with passed waypoints marked")
     };
   }
 
@@ -58,6 +63,7 @@ class RemovePassedGoals : public BT::ActionNodeBase
 
   double viapoint_achieved_radius_;
   double transform_tolerance_;
+  rclcpp::Node::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string robot_base_frame_;
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -102,7 +102,9 @@
       <input_port name="input_goals">Input goals to remove if passed</input_port>
       <input_port name="radius">Radius tolerance on a goal to consider it passed</input_port>
       <input_port name="robot_base_frame">Robot base frame</input_port>
+      <input_port name="input_waypoint_statuses">Original waypoint_statuses to mark waypoint status from</input_port>
       <output_port name="output_goals">Set of goals after removing any passed</output_port>
+      <output_port name="output_waypoint_statuses">Waypoint_statuses with passed waypoints marked</output_port>
     </Action>
 
     <Action ID="RemoveInCollisionGoals">
@@ -111,7 +113,9 @@
       <input_port name="use_footprint">Whether to use the footprint cost or the point cost.</input_port>
       <input_port name="cost_threshold">The cost threshold above which a waypoint is considered in collision and should be removed.</input_port>
       <input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
+      <input_port name="input_waypoint_statuses">Original waypoint_statuses to mark waypoint status from</input_port>
       <output_port name="output_goals">A vector of goals containing only those that are not in collision.</output_port>
+      <output_port name="output_waypoint_statuses">Waypoint_statuses with in-collision waypoints marked</output_port>
     </Action>
 
     <Action ID="SmoothPath">

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -20,6 +20,7 @@
 #include "nav2_behavior_tree/bt_utils.hpp"
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -63,12 +64,29 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
     return BT::NodeStatus::FAILURE;
   }
 
+  // get the `waypoint_statuses` vector
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
+  auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
+  if (!waypoint_statuses_get_res) {
+    RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
+  }
+
   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 (waypoint_statuses_get_res) {
+      using namespace nav2_util::geometry_utils;  // NOLINT
+      auto cur_waypoint_index =
+        find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, input_goals_.goals[i]);
+      if (cur_waypoint_index == -1) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
+        return BT::NodeStatus::FAILURE;
+      }
+      waypoint_statuses[cur_waypoint_index].waypoint_status =
+        nav2_msgs::msg::WaypointStatus::SKIPPED;
     }
   }
   // Inform if all goals have been removed
@@ -78,6 +96,9 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
       "All goals are in collision and have been removed from the list");
   }
   setOutput("output_goals", valid_goal_poses);
+  // set `waypoint_statuses` output
+  setOutput("output_waypoint_statuses", waypoint_statuses);
+
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

@@ -36,11 +36,11 @@ void RemovePassedGoals::initialize()
   getInput("radius", viapoint_achieved_radius_);
 
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
-  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-  node->get_parameter("transform_tolerance", transform_tolerance_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
 
   robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
-    node, "robot_base_frame", this);
+    node_, "robot_base_frame", this);
 }
 
 inline BT::NodeStatus RemovePassedGoals::tick()
@@ -67,6 +67,13 @@ inline BT::NodeStatus RemovePassedGoals::tick()
     return BT::NodeStatus::FAILURE;
   }
 
+  // get the `waypoint_statuses` vector
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
+  auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
+  if (!waypoint_statuses_get_res) {
+    RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
+  }
+
   double dist_to_goal;
   while (goal_poses.goals.size() > 1) {
     dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose);
@@ -75,10 +82,24 @@ inline BT::NodeStatus RemovePassedGoals::tick()
       break;
     }
 
+    // mark waypoint statuses before the goal is erased from goals
+    if (waypoint_statuses_get_res) {
+      auto cur_waypoint_index =
+        find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[0]);
+      if (cur_waypoint_index == -1) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
+        return BT::NodeStatus::FAILURE;
+      }
+      waypoint_statuses[cur_waypoint_index].waypoint_status =
+        nav2_msgs::msg::WaypointStatus::COMPLETED;
+    }
+
     goal_poses.goals.erase(goal_poses.goals.begin());
   }
 
   setOutput("output_goals", goal_poses);
+  // set `waypoint_statuses` output
+  setOutput("output_waypoint_statuses", waypoint_statuses);
 
   return BT::NodeStatus::SUCCESS;
 }

nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp

@@ -181,6 +181,139 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su
   EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
 }
 
+TEST_F(RemoveInCollisionGoalsTestFixture,
+  test_tick_remove_in_collision_goals_success_and_output_waypoint_statues)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap"
+                                  input_goals="{goals}" output_goals="{goals}"
+                                  cost_threshold="253"
+                                  input_waypoint_statuses="{waypoint_statuses}"
+                                  output_waypoint_statuses="{waypoint_statuses}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Goals poses;
+  poses.goals.resize(4);
+  poses.goals[0].pose.position.x = 0.0;
+  poses.goals[0].pose.position.y = 0.0;
+
+  poses.goals[1].pose.position.x = 0.5;
+  poses.goals[1].pose.position.y = 0.0;
+
+  poses.goals[2].pose.position.x = 1.0;
+  poses.goals[2].pose.position.y = 0.0;
+
+  poses.goals[3].pose.position.x = 2.0;
+  poses.goals[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // create waypoint_statuses and set it on blackboard
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
+  for (size_t i = 0 ; i < waypoint_statuses.size() ; ++i) {
+    waypoint_statuses[i].waypoint_pose = poses.goals[i];
+    waypoint_statuses[i].waypoint_index = i;
+  }
+  config_->blackboard->set("waypoint_statuses", waypoint_statuses);
+
+  // tick until node is not running
+  tree_->rootNode()->executeTick();
+  while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  // check that it removed the point in range
+  nav_msgs::msg::Goals output_poses;
+  EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
+
+  EXPECT_EQ(output_poses.goals.size(), 3u);
+  EXPECT_EQ(output_poses.goals[0], poses.goals[0]);
+  EXPECT_EQ(output_poses.goals[1], poses.goals[1]);
+  EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
+
+  // check the waypoint_statuses
+  std::vector<nav2_msgs::msg::WaypointStatus> output_waypoint_statuses;
+  EXPECT_TRUE(config_->blackboard->get("waypoint_statuses", output_waypoint_statuses));
+  EXPECT_EQ(output_waypoint_statuses.size(), 4u);
+  EXPECT_EQ(output_waypoint_statuses[0].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+  EXPECT_EQ(output_waypoint_statuses[1].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+  EXPECT_EQ(output_waypoint_statuses[2].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+  EXPECT_EQ(output_waypoint_statuses[3].waypoint_status, nav2_msgs::msg::WaypointStatus::SKIPPED);
+}
+
+TEST_F(RemoveInCollisionGoalsTestFixture,
+  test_tick_remove_in_collision_goals_find_matching_waypoint_fail)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap"
+                                  input_goals="{goals}" output_goals="{goals}"
+                                  cost_threshold="253"
+                                  input_waypoint_statuses="{waypoint_statuses}"
+                                  output_waypoint_statuses="{waypoint_statuses}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Goals poses;
+  poses.goals.resize(4);
+  poses.goals[0].pose.position.x = 0.0;
+  poses.goals[0].pose.position.y = 0.0;
+
+  poses.goals[1].pose.position.x = 0.5;
+  poses.goals[1].pose.position.y = 0.0;
+
+  poses.goals[2].pose.position.x = 1.0;
+  poses.goals[2].pose.position.y = 0.0;
+
+  poses.goals[3].pose.position.x = 2.0;
+  poses.goals[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // create waypoint_statuses and set it on blackboard
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
+  for (size_t i = 0 ; i < waypoint_statuses.size() ; ++i) {
+    waypoint_statuses[i].waypoint_pose = poses.goals[i];
+    waypoint_statuses[i].waypoint_index = i;
+  }
+  // inconsistency between waypoint_statuses and poses
+  waypoint_statuses[3].waypoint_pose.pose.position.x = 0.0;
+
+  config_->blackboard->set("waypoint_statuses", waypoint_statuses);
+
+  // tick until node is not running
+  tree_->rootNode()->executeTick();
+  while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check that it failed and returned the original goals
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  nav_msgs::msg::Goals output_poses;
+  EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
+
+  EXPECT_EQ(output_poses.goals.size(), 4u);
+  EXPECT_EQ(output_poses.goals[0], poses.goals[0]);
+  EXPECT_EQ(output_poses.goals[1], poses.goals[1]);
+  EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
+  EXPECT_EQ(output_poses.goals[3], poses.goals[3]);
+}
+
 TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail)
 {
   // create tree