Add validation tests

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

Author: zz990099

nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp

@@ -181,6 +181,75 @@ 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::MISSED);
+}
+
 TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail)
 {
   // create tree

nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp

@@ -39,6 +39,7 @@ class RemovePassedGoalsTestFixture : public ::testing::Test
 
     config_ = new BT::NodeConfiguration();
     transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(node_);
+    transform_handler_->activate();
 
     // Create the blackboard that will be shared by all of the nodes in the tree
     config_->blackboard = BT::Blackboard::create();
@@ -98,7 +99,6 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick)
   pose.position.x = 0.25;
   pose.position.y = 0.0;
 
-  transform_handler_->activate();
   transform_handler_->waitForTransform();
   transform_handler_->updateRobotPose(pose);
 
@@ -144,6 +144,77 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick)
   EXPECT_EQ(output_poses.goals[1], poses.goals[3]);
 }
 
+TEST_F(RemovePassedGoalsTestFixture,
+  test_tick_remove_passed_goals_success_and_output_waypoint_statuses)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.25;
+  pose.position.y = 0.0;
+
+  transform_handler_->waitForTransform();
+  transform_handler_->updateRobotPose(pose);
+
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemovePassedGoals radius="0.5" input_goals="{goals}" output_goals="{goals}"
+                             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 succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // 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(), 2u);
+  EXPECT_EQ(output_poses.goals[0], poses.goals[2]);
+  EXPECT_EQ(output_poses.goals[1], poses.goals[3]);
+
+  // 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::COMPLETED);
+  EXPECT_EQ(output_waypoint_statuses[1].waypoint_status, nav2_msgs::msg::WaypointStatus::COMPLETED);
+  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::PENDING);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_util/test/test_geometry_utils.cpp

@@ -21,6 +21,7 @@
 
 using nav2_util::geometry_utils::euclidean_distance;
 using nav2_util::geometry_utils::calculate_path_length;
+using nav2_util::geometry_utils::find_next_matching_goal_in_waypoint_statuses;
 
 TEST(GeometryUtils, euclidean_distance_point_3d)
 {
@@ -128,3 +129,30 @@ TEST(GeometryUtils, calculate_path_length)
     calculate_path_length(circle_path),
     2 * pi * polar_distance, 1e-1);
 }
+
+TEST(GeometryUtils, find_next_matching_goal_in_waypoint_statuses)
+{
+  size_t nb_waypoints = 10;
+  float distance_between_waypoints = 2.0;
+
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(nb_waypoints);
+  for (size_t i = 0 ; i < nb_waypoints ; ++i) {
+    waypoint_statuses[i].waypoint_index = i;
+    waypoint_statuses[i].waypoint_pose.pose.position.x = distance_between_waypoints * i;
+  }
+
+  // match success
+  size_t matching_index = nb_waypoints / 2;
+  ASSERT_EQ(find_next_matching_goal_in_waypoint_statuses(waypoint_statuses,
+    waypoint_statuses[matching_index].waypoint_pose), matching_index);
+
+  // match failed due to mismatch of pose
+  geometry_msgs::msg::PoseStamped fake_pose;
+  fake_pose.pose.position.x = -1.0;
+  ASSERT_EQ(find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, fake_pose), -1);
+
+  // match failed due to waypoint_status is not PENDING
+  waypoint_statuses[matching_index].waypoint_status = nav2_msgs::msg::WaypointStatus::COMPLETED;
+  ASSERT_EQ(find_next_matching_goal_in_waypoint_statuses(waypoint_statuses,
+    waypoint_statuses[matching_index].waypoint_pose), -1);
+}