From 52dedc47c03a84486d4209bdca7d51a5d33a3cf9 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Thu, 26 Dec 2024 14:35:11 +0900
Subject: [PATCH] refactor(behavior_velocity_planner): independent of plugin
 packages (#9760)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../CMakeLists.txt                            |   3 +-
 .../behavior_velocity_planner}/node.hpp       |   8 +-
 .../planner_manager.hpp                       |   6 +-
 .../behavior_velocity_planner/test_utils.hpp  |  47 ++++++++
 .../package.xml                               |  13 ---
 .../src/node.cpp                              |   2 +-
 .../src/planner_manager.cpp                   |   2 +-
 .../test_utils.cpp}                           | 108 ++++--------------
 .../test/test_node_interface.cpp              |  61 ++++++++++
 9 files changed, 144 insertions(+), 106 deletions(-)
 rename planning/behavior_velocity_planner/autoware_behavior_velocity_planner/{src => include/autoware/behavior_velocity_planner}/node.hpp (96%)
 rename planning/behavior_velocity_planner/autoware_behavior_velocity_planner/{src => include/autoware/behavior_velocity_planner}/planner_manager.hpp (91%)
 create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp
 rename planning/behavior_velocity_planner/autoware_behavior_velocity_planner/{test/src/test_node_interface.cpp => src/test_utils.cpp} (50%)
 create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt
index 37a02b844dfe9..31900a23d00e5 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt
@@ -16,6 +16,7 @@ autoware_package()
 ament_auto_add_library(${PROJECT_NAME}_lib SHARED
   src/node.cpp
   src/planner_manager.cpp
+  src/test_utils.cpp
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}_lib
@@ -34,7 +35,7 @@ endif()
 
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
-    test/src/test_node_interface.cpp
+    test/test_node_interface.cpp
   )
   target_link_libraries(test_${PROJECT_NAME}
     gtest_main
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp
similarity index 96%
rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp
rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp
index 4efb58e38b74f..4c1f10c355226 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NODE_HPP_
-#define NODE_HPP_
+#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_
+#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_
 
+#include "autoware/behavior_velocity_planner/planner_manager.hpp"
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 #include "autoware/universe_utils/ros/polling_subscriber.hpp"
-#include "planner_manager.hpp"
 
 #include <autoware/behavior_velocity_planner_common/planner_data.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
@@ -150,4 +150,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
 };
 }  // namespace autoware::behavior_velocity_planner
 
-#endif  // NODE_HPP_
+#endif  // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp
similarity index 91%
rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp
rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp
index fddd658cef06e..9bd423ecfef21 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PLANNER_MANAGER_HPP_
-#define PLANNER_MANAGER_HPP_
+#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_
+#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_
 
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
@@ -57,4 +57,4 @@ class BehaviorVelocityPlannerManager
 };
 }  // namespace autoware::behavior_velocity_planner
 
-#endif  // PLANNER_MANAGER_HPP_
+#endif  // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp
new file mode 100644
index 0000000000000..f34182e77e6f1
--- /dev/null
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp
@@ -0,0 +1,47 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_
+#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_
+
+#include "autoware/behavior_velocity_planner/node.hpp"
+
+#include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace autoware::behavior_velocity_planner
+{
+using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode;
+using autoware::planning_test_manager::PlanningInterfaceTestManager;
+
+struct PluginInfo
+{
+  std::string module_name;  // e.g. crosswalk
+  std::string plugin_name;  // e.g. autoware::behavior_velocity_planner::CrosswalkModulePlugin
+};
+
+std::shared_ptr<PlanningInterfaceTestManager> generateTestManager();
+
+std::shared_ptr<BehaviorVelocityPlannerNode> generateNode(
+  const std::vector<PluginInfo> & plugin_info_vec);
+
+void publishMandatoryTopics(
+  std::shared_ptr<PlanningInterfaceTestManager> test_manager,
+  std::shared_ptr<BehaviorVelocityPlannerNode> test_target_node);
+}  // namespace autoware::behavior_velocity_planner
+
+#endif  // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml
index e051374ed3dda..04b4cf0328fd1 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml
@@ -64,19 +64,6 @@
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>autoware_behavior_velocity_blind_spot_module</test_depend>
-  <test_depend>autoware_behavior_velocity_crosswalk_module</test_depend>
-  <test_depend>autoware_behavior_velocity_detection_area_module</test_depend>
-  <test_depend>autoware_behavior_velocity_intersection_module</test_depend>
-  <test_depend>autoware_behavior_velocity_no_drivable_lane_module</test_depend>
-  <test_depend>autoware_behavior_velocity_no_stopping_area_module</test_depend>
-  <test_depend>autoware_behavior_velocity_occlusion_spot_module</test_depend>
-  <test_depend>autoware_behavior_velocity_run_out_module</test_depend>
-  <test_depend>autoware_behavior_velocity_speed_bump_module</test_depend>
-  <test_depend>autoware_behavior_velocity_stop_line_module</test_depend>
-  <test_depend>autoware_behavior_velocity_traffic_light_module</test_depend>
-  <test_depend>autoware_behavior_velocity_virtual_traffic_light_module</test_depend>
-  <test_depend>autoware_behavior_velocity_walkway_module</test_depend>
   <test_depend>autoware_lint_common</test_depend>
   <!--<test_depend>autoware_behavior_velocity_template_module</test_depend>-->
 
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp
index d78bc883e6b35..5f78e4c670b49 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "node.hpp"
+#include "autoware/behavior_velocity_planner/node.hpp"
 
 #include <autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <autoware/motion_utils/trajectory/path_with_lane_id.hpp>
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp
index 4820c340058ff..45ee83260d53a 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "planner_manager.hpp"
+#include "autoware/behavior_velocity_planner/planner_manager.hpp"
 
 #include <autoware/motion_utils/trajectory/interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp
similarity index 50%
rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp
rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp
index fe79450d0def8..ee1bb8f89fc46 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,22 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "node.hpp"
+#include "autoware/behavior_velocity_planner/test_utils.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
 #include <autoware_test_utils/autoware_test_utils.hpp>
 
-#include <gtest/gtest.h>
-
-#include <cmath>
 #include <memory>
 #include <string>
 #include <vector>
 
-using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode;
-using autoware::planning_test_manager::PlanningInterfaceTestManager;
-
+namespace autoware::behavior_velocity_planner
+{
 std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
 {
   auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
@@ -45,7 +41,8 @@ std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
   return test_manager;
 }
 
-std::shared_ptr<BehaviorVelocityPlannerNode> generateNode()
+std::shared_ptr<BehaviorVelocityPlannerNode> generateNode(
+  const std::vector<PluginInfo> & plugin_info_vec)
 {
   auto node_options = rclcpp::NodeOptions{};
 
@@ -64,49 +61,29 @@ std::shared_ptr<BehaviorVelocityPlannerNode> generateNode()
     return package_path + "/config/" + module + ".param.yaml";
   };
 
-  std::vector<std::string> module_names;
-  module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin");
-  module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin");
+  std::vector<std::string> plugin_names;
+  for (const auto & plugin_info : plugin_info_vec) {
+    plugin_names.emplace_back(plugin_info.plugin_name);
+  }
 
   std::vector<rclcpp::Parameter> params;
-  params.emplace_back("launch_modules", module_names);
+  params.emplace_back("launch_modules", plugin_names);
   params.emplace_back("is_simulation", false);
   node_options.parameter_overrides(params);
 
-  autoware::test_utils::updateNodeOptions(
-    node_options,
-    {autoware_test_utils_dir + "/config/test_common.param.yaml",
-     autoware_test_utils_dir + "/config/test_nearest_search.param.yaml",
-     autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml",
-     velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml",
-     velocity_smoother_dir + "/config/Analytical.param.yaml",
-     behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml",
-     behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml",
-     get_behavior_velocity_module_config("blind_spot"),
-     get_behavior_velocity_module_config("crosswalk"),
-     get_behavior_velocity_module_config("walkway"),
-     get_behavior_velocity_module_config("detection_area"),
-     get_behavior_velocity_module_config("intersection"),
-     get_behavior_velocity_module_config("no_stopping_area"),
-     get_behavior_velocity_module_config("occlusion_spot"),
-     get_behavior_velocity_module_config("run_out"),
-     get_behavior_velocity_module_config("speed_bump"),
-     get_behavior_velocity_module_config("stop_line"),
-     get_behavior_velocity_module_config("traffic_light"),
-     get_behavior_velocity_module_config("virtual_traffic_light"),
-     get_behavior_velocity_module_config("no_drivable_lane")});
+  auto yaml_files = std::vector<std::string>{
+    autoware_test_utils_dir + "/config/test_common.param.yaml",
+    autoware_test_utils_dir + "/config/test_nearest_search.param.yaml",
+    autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml",
+    velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml",
+    velocity_smoother_dir + "/config/Analytical.param.yaml",
+    behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml",
+    behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml"};
+  for (const auto & plugin_info : plugin_info_vec) {
+    yaml_files.push_back(get_behavior_velocity_module_config(plugin_info.module_name));
+  }
+
+  autoware::test_utils::updateNodeOptions(node_options, yaml_files);
 
   // TODO(Takagi, Isamu): set launch_modules
   // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light
@@ -141,39 +118,4 @@ void publishMandatoryTopics(
   test_manager->publishOccupancyGrid(
     test_target_node, "behavior_velocity_planner_node/input/occupancy_grid");
 }
-
-TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
-{
-  rclcpp::init(0, nullptr);
-  auto test_manager = generateTestManager();
-  auto test_target_node = generateNode();
-
-  publishMandatoryTopics(test_manager, test_target_node);
-
-  // test with nominal path_with_lane_id
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
-  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));
-  rclcpp::shutdown();
-}
-
-TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
-{
-  rclcpp::init(0, nullptr);
-
-  auto test_manager = generateTestManager();
-  auto test_target_node = generateNode();
-  publishMandatoryTopics(test_manager, test_target_node);
-
-  // test for normal trajectory
-  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
-
-  // 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));
-
-  rclcpp::shutdown();
-}
+}  // namespace autoware::behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp
new file mode 100644
index 0000000000000..6e232318c1711
--- /dev/null
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp
@@ -0,0 +1,61 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/behavior_velocity_planner/test_utils.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace autoware::behavior_velocity_planner
+{
+TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
+{
+  rclcpp::init(0, nullptr);
+  auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
+  auto test_target_node = autoware::behavior_velocity_planner::generateNode({});
+
+  autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
+
+  // test with nominal path_with_lane_id
+  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
+  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));
+  rclcpp::shutdown();
+}
+
+TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
+{
+  rclcpp::init(0, nullptr);
+
+  auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
+  auto test_target_node = autoware::behavior_velocity_planner::generateNode({});
+  autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
+
+  // test for normal trajectory
+  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
+
+  // 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));
+
+  rclcpp::shutdown();
+}
+}  // namespace autoware::behavior_velocity_planner