adapt behavior_velocity_planner to new test manager

Signed-off-by: mitukou1109 <[email protected]>
autowarefoundation · Jan 17, 2025 · 8dd0be2 · 8dd0be2
1 parent b8eb8fc
commit 8dd0be2
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 28 deletions.
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp
@@ -29,14 +29,8 @@ std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
   auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
 
   // set subscriber with topic name: behavior_velocity_planner → test_node_
-  test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path");
-
-  // set behavior_velocity_planner node's input topic name(this topic is changed to test node)
-  test_manager->setPathWithLaneIdTopicName(
-    "behavior_velocity_planner_node/input/path_with_lane_id");
-
-  test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry");
-  test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry");
+  test_manager->subscribeOutput<autoware_planning_msgs::msg::Path>(
+    "behavior_velocity_planner_node/output/path");
 
   return test_manager;
 }
@@ -99,21 +93,35 @@ void publishMandatoryTopics(
   std::shared_ptr<BehaviorVelocityPlannerNode> test_target_node)
 {
   // publish necessary topics from test_manager
-  test_manager->publishTF(test_target_node, "/tf");
-  test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel");
-  test_manager->publishPredictedObjects(
-    test_target_node, "behavior_velocity_planner_node/input/dynamic_objects");
-  test_manager->publishPointCloud(
-    test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud");
-  test_manager->publishOdometry(
-    test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry");
-  test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel");
-  test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map");
-  test_manager->publishTrafficSignals(
-    test_target_node, "behavior_velocity_planner_node/input/traffic_signals");
-  test_manager->publishMaxVelocity(
-    test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps");
-  test_manager->publishOccupancyGrid(
-    test_target_node, "behavior_velocity_planner_node/input/occupancy_grid");
+  test_manager->publishInput(
+    test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map"));
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/accel",
+    geometry_msgs::msg::AccelWithCovarianceStamped{});
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/dynamic_objects",
+    autoware_perception_msgs::msg::PredictedObjects{});
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud",
+    sensor_msgs::msg::PointCloud2{}.set__header(
+      std_msgs::msg::Header{}.set__frame_id("base_link")));
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry",
+    autoware::test_utils::makeOdometry());
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/accel",
+    geometry_msgs::msg::AccelWithCovarianceStamped{});
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/vector_map",
+    autoware::test_utils::makeMapBinMsg());
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/traffic_signals",
+    autoware_perception_msgs::msg::TrafficLightGroupArray{});
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps",
+    tier4_planning_msgs::msg::VelocityLimit{});
+  test_manager->publishInput(
+    test_target_node, "behavior_velocity_planner_node/input/occupancy_grid",
+    autoware::test_utils::makeCostMapMsg());
 }
 }  // namespace autoware::behavior_velocity_planner
diff --git a/...behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/...behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp
@@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
 
   autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
 
+  const std::string input_path_with_lane_id_topic =
+    "behavior_velocity_planner_node/input/path_with_lane_id";
+
   // test with nominal path_with_lane_id
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic));
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test with empty path_with_lane_id
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic));
   rclcpp::shutdown();
 }
 
@@ -48,13 +53,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
   auto test_target_node = autoware::behavior_velocity_planner::generateNode({});
   autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
 
+  const std::string input_path_with_lane_id_topic =
+    "behavior_velocity_planner_node/input/path_with_lane_id";
+  const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry";
+
   // test for normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic));
 
   // make sure behavior_path_planner is running
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic));
 
   rclcpp::shutdown();
 }