Merge pull request #1787 from tier4/cherry-pick/9882

feat(planning_test_manager): abstract message-specific functions (autowarefoundation#9882)

planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp

@@ -21,6 +21,7 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <string>
 #include <vector>
 
 using autoware::freespace_planner::FreespacePlannerNode;
@@ -29,10 +30,8 @@ using autoware::planning_test_manager::PlanningInterfaceTestManager;
 std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
 {
   auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
-  test_manager->setRouteInputTopicName("freespace_planner/input/route");
-  test_manager->setTrajectorySubscriber("freespace_planner/output/trajectory");
-  test_manager->setOdometryTopicName("freespace_planner/input/odometry");
-  test_manager->setInitialPoseTopicName("freespace_planner/input/odometry");
+  test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
+    "freespace_planner/output/trajectory");
   return test_manager;
 }
 
@@ -55,10 +54,16 @@ void publishMandatoryTopics(
   rclcpp::Node::SharedPtr test_target_node)
 {
   // publish necessary topics from test_manager
-  test_manager->publishTF(test_target_node, "/tf");
-  test_manager->publishOdometry(test_target_node, "freespace_planner/input/odometry");
-  test_manager->publishOccupancyGrid(test_target_node, "freespace_planner/input/occupancy_grid");
-  test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario");
+  test_manager->publishInput(
+    test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map"));
+  test_manager->publishInput(
+    test_target_node, "freespace_planner/input/odometry", autoware::test_utils::makeOdometry());
+  test_manager->publishInput(
+    test_target_node, "freespace_planner/input/occupancy_grid",
+    autoware::test_utils::makeCostMapMsg());
+  test_manager->publishInput(
+    test_target_node, "freespace_planner/input/scenario",
+    autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::PARKING));
 }
 
 // the following tests are disable because they randomly fail
@@ -70,12 +75,16 @@ TEST(PlanningModuleInterfaceTest, DISABLED_testPlanningInterfaceWithVariousTraje
   auto test_target_node = generateNode();
   publishMandatoryTopics(test_manager, test_target_node);
 
+  const std::string input_route_topic = "freespace_planner/input/route";
+
   // test with normal route
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic));
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test with empty route
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithAbnormalRoute(test_target_node, input_route_topic));
   rclcpp::shutdown();
 }
 
@@ -87,11 +96,16 @@ TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose)
 
   publishMandatoryTopics(test_manager, test_target_node);
 
+  const std::string input_route_topic = "freespace_planner/input/route";
+  const std::string input_odometry_topic = "freespace_planner/input/odometry";
+
   // test for normal route
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic));
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));
 
   rclcpp::shutdown();
 }

planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp

@@ -21,6 +21,7 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <string>
 #include <vector>
 
 using autoware::motion_planning::ObstacleCruisePlannerNode;
@@ -31,12 +32,8 @@ std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
   auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
 
   // set subscriber with topic name: obstacle_cruise_planner → test_node_
-  test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory");
-
-  // set obstacle_cruise_planners input topic name(this topic is changed to test node):
-  test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory");
-
-  test_manager->setOdometryTopicName("obstacle_cruise_planner/input/odometry");
+  test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
+    "obstacle_cruise_planner/output/trajectory");
 
   return test_manager;
 }
@@ -63,9 +60,15 @@ void publishMandatoryTopics(
   std::shared_ptr<ObstacleCruisePlannerNode> test_target_node)
 {
   // publish necessary topics from test_manager
-  test_manager->publishOdometry(test_target_node, "obstacle_cruise_planner/input/odometry");
-  test_manager->publishPredictedObjects(test_target_node, "obstacle_cruise_planner/input/objects");
-  test_manager->publishAcceleration(test_target_node, "obstacle_cruise_planner/input/acceleration");
+  test_manager->publishInput(
+    test_target_node, "obstacle_cruise_planner/input/odometry",
+    autoware::test_utils::makeOdometry());
+  test_manager->publishInput(
+    test_target_node, "obstacle_cruise_planner/input/objects",
+    autoware_perception_msgs::msg::PredictedObjects{});
+  test_manager->publishInput(
+    test_target_node, "obstacle_cruise_planner/input/acceleration",
+    geometry_msgs::msg::AccelWithCovarianceStamped{});
 }
 
 TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -76,12 +79,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
 
   publishMandatoryTopics(test_manager, test_target_node);
 
+  const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory";
+
   // test for normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test for trajectory with empty/one point/overlapping point
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic));
 
   rclcpp::shutdown();
 }
@@ -94,12 +101,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
 
   publishMandatoryTopics(test_manager, test_target_node);
 
+  const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory";
+  const std::string input_odometry_topic = "obstacle_cruise_planner/input/odometry";
+
   // test for normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test for trajectory with empty/one point/overlapping point
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));
 
   rclcpp::shutdown();
 }

planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp

@@ -23,6 +23,7 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <string>
 #include <vector>
 
 using autoware::motion_planning::ObstacleStopPlannerNode;
@@ -32,9 +33,8 @@ using tier4_planning_msgs::msg::ExpandStopRange;
 std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
 {
   auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
-  test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory");
-  test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory");
-  test_manager->setOdometryTopicName("obstacle_stop_planner/input/odometry");
+  test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
+    "obstacle_stop_planner/output/trajectory");
   return test_manager;
 }
 
@@ -65,21 +65,21 @@ void publishMandatoryTopics(
   std::shared_ptr<ObstacleStopPlannerNode> test_target_node)
 {
   // publish necessary topics from test_manager
-  test_manager->publishOdometry(test_target_node, "obstacle_stop_planner/input/odometry");
-  test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud");
-  test_manager->publishAcceleration(test_target_node, "obstacle_stop_planner/input/acceleration");
-  test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects");
-}
-
-void publishExpandStopRange(
-  std::shared_ptr<PlanningInterfaceTestManager> test_manager,
-  std::shared_ptr<ObstacleStopPlannerNode> test_target_node)
-{
-  auto test_node = test_manager->getTestNode();
-  const auto expand_stop_range = test_manager->getTestNode()->create_publisher<ExpandStopRange>(
-    "obstacle_stop_planner/input/expand_stop_range", 1);
-  expand_stop_range->publish(ExpandStopRange{});
-  autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3);
+  test_manager->publishInput(
+    test_target_node, "obstacle_stop_planner/input/odometry", autoware::test_utils::makeOdometry());
+  test_manager->publishInput(
+    test_target_node, "obstacle_stop_planner/input/pointcloud",
+    sensor_msgs::msg::PointCloud2{}.set__header(
+      std_msgs::msg::Header{}.set__frame_id("base_link")));
+  test_manager->publishInput(
+    test_target_node, "obstacle_stop_planner/input/acceleration",
+    geometry_msgs::msg::AccelWithCovarianceStamped{});
+  test_manager->publishInput(
+    test_target_node, "obstacle_stop_planner/input/objects",
+    autoware_perception_msgs::msg::PredictedObjects{});
+  test_manager->publishInput(
+    test_target_node, "obstacle_stop_planner/input/expand_stop_range",
+    tier4_planning_msgs::msg::ExpandStopRange{});
 }
 
 TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -90,15 +90,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
   auto test_target_node = generateNode();
 
   publishMandatoryTopics(test_manager, test_target_node);
-  publishExpandStopRange(test_manager, test_target_node);
+
+  const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory";
 
   // test for normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
 
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test for trajectory with empty/one point/overlapping point
-  test_manager->testWithAbnormalTrajectory(test_target_node);
+  test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic);
 
   rclcpp::shutdown();
 }
@@ -110,14 +112,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
   auto test_target_node = generateNode();
 
   publishMandatoryTopics(test_manager, test_target_node);
-  publishExpandStopRange(test_manager, test_target_node);
+
+  const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory";
+  const std::string input_odometry_topic = "obstacle_stop_planner/input/odometry";
 
   // test for normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test for trajectory with empty/one point/overlapping point
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));
 
   rclcpp::shutdown();
 }

planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp

@@ -21,6 +21,7 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <string>
 #include <vector>
 
 TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -47,21 +48,24 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
   auto test_target_node = std::make_shared<autoware::path_optimizer::PathOptimizer>(node_options);
 
   // publish necessary topics from test_manager
-  test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry");
+  test_manager->publishInput(
+    test_target_node, "path_optimizer/input/odometry", autoware::test_utils::makeOdometry());
 
   // set subscriber with topic name: path_optimizer → test_node_
-  test_manager->setTrajectorySubscriber("path_optimizer/output/path");
+  test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
+    "path_optimizer/output/path");
 
-  // set path_optimizer's input topic name(this topic is changed to test node)
-  test_manager->setPathInputTopicName("path_optimizer/input/path");
+  const std::string input_trajectory_topic = "path_optimizer/input/path";
 
   // test with normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalPath(test_target_node, input_trajectory_topic));
 
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test with trajectory with empty/one point/overlapping point
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithAbnormalPath(test_target_node, input_trajectory_topic));
 
   rclcpp::shutdown();
 }

planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp

@@ -21,6 +21,7 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <string>
 #include <vector>
 
 TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -46,22 +47,27 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
     std::make_shared<autoware::path_smoother::ElasticBandSmoother>(node_options);
 
   // publish necessary topics from test_manager
-  test_manager->publishOdometry(test_target_node, "autoware_path_smoother/input/odometry");
+  test_manager->publishInput(
+    test_target_node, "autoware_path_smoother/input/odometry",
+    autoware::test_utils::makeOdometry());
 
   // set subscriber with topic name
-  test_manager->setTrajectorySubscriber("autoware_path_smoother/output/traj");
-  test_manager->setPathSubscriber("autoware_path_smoother/output/path");
+  test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
+    "autoware_path_smoother/output/traj");
+  test_manager->subscribeOutput<autoware_planning_msgs::msg::Path>(
+    "autoware_path_smoother/output/path");
 
-  // set input topic name (this topic is changed to test node)
-  test_manager->setPathInputTopicName("autoware_path_smoother/input/path");
+  const std::string input_path_topic = "autoware_path_smoother/input/path";
 
   // test with normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithNormalPath(test_target_node, input_path_topic));
 
   EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
 
   // test with trajectory with empty/one point/overlapping point
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node));
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithAbnormalPath(test_target_node, input_path_topic));
 
   rclcpp::shutdown();
 }