feat(behavior_velocity_crosswalk): add node test

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt

@@ -15,6 +15,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
   ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
     test/test_crosswalk.cpp
+    test/test_node_interface.cpp
   )
   target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
 endif()

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml

@@ -21,6 +21,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>autoware_behavior_velocity_planner</depend>
   <depend>autoware_behavior_velocity_planner_common</depend>
   <depend>autoware_grid_map_utils</depend>
   <depend>autoware_interpolation</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp

@@ -0,0 +1,64 @@
+// 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.
+
+#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);
+
+  const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{
+    "crosswalk", "autoware::behavior_velocity_planner::CrosswalkModulePlugin"}};
+
+  auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
+  auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec);
+  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

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt

@@ -2,39 +2,23 @@ cmake_minimum_required(VERSION 3.14)
 project(autoware_behavior_velocity_planner)
 
 find_package(autoware_cmake REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-rosidl_generate_interfaces(
-  ${PROJECT_NAME}
-  "srv/LoadPlugin.srv"
-  "srv/UnloadPlugin.srv"
-  DEPENDENCIES
-)
 
 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
   PLUGIN "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode"
   EXECUTABLE ${PROJECT_NAME}_node
 )
 
-if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
-    rosidl_target_interfaces(${PROJECT_NAME}_lib
-    ${PROJECT_NAME} "rosidl_typesupport_cpp")
-else()
-    rosidl_get_typesupport_target(
-            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
-    target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}")
-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

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp → planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp

@@ -12,19 +12,18 @@
 // 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>
-#include <autoware_behavior_velocity_planner/srv/load_plugin.hpp>
-#include <autoware_behavior_velocity_planner/srv/unload_plugin.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_internal_debug_msgs/srv/string.hpp>
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/path.hpp>
@@ -45,8 +44,6 @@
 
 namespace autoware::behavior_velocity_planner
 {
-using autoware_behavior_velocity_planner::srv::LoadPlugin;
-using autoware_behavior_velocity_planner::srv::UnloadPlugin;
 using autoware_map_msgs::msg::LaneletMapBin;
 using tier4_planning_msgs::msg::VelocityLimit;
 
@@ -125,13 +122,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
   BehaviorVelocityPlannerManager planner_manager_;
   bool is_driving_forward_{true};
 
-  rclcpp::Service<LoadPlugin>::SharedPtr srv_load_plugin_;
-  rclcpp::Service<UnloadPlugin>::SharedPtr srv_unload_plugin_;
+  rclcpp::Service<autoware_internal_debug_msgs::srv::String>::SharedPtr srv_load_plugin_;
+  rclcpp::Service<autoware_internal_debug_msgs::srv::String>::SharedPtr srv_unload_plugin_;
   void onUnloadPlugin(
-    const UnloadPlugin::Request::SharedPtr request,
-    const UnloadPlugin::Response::SharedPtr response);
+    const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request,
+    const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response);
   void onLoadPlugin(
-    const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response);
+    const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request,
+    const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response);
 
   // mutex for planner_data_
   std::mutex mutex_;
@@ -150,4 +148,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
 };
 }  // namespace autoware::behavior_velocity_planner
 
-#endif  // NODE_HPP_
+#endif  // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp → 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_

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_

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml

@@ -32,9 +32,8 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
-  <build_depend>rosidl_default_generators</build_depend>
-
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_motion_utils</depend>
@@ -60,28 +59,11 @@
   <depend>tier4_v2x_msgs</depend>
   <depend>visualization_msgs</depend>
 
-  <exec_depend>rosidl_default_runtime</exec_depend>
-
   <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>-->
 
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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>
@@ -70,9 +70,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
     this->create_subscription<tier4_planning_msgs::msg::PathWithLaneId>(
       "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1));
 
-  srv_load_plugin_ = create_service<LoadPlugin>(
+  srv_load_plugin_ = create_service<autoware_internal_debug_msgs::srv::String>(
     "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2));
-  srv_unload_plugin_ = create_service<UnloadPlugin>(
+  srv_unload_plugin_ = create_service<autoware_internal_debug_msgs::srv::String>(
     "~/service/unload_plugin",
     std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2));
 
@@ -112,19 +112,19 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
 }
 
 void BehaviorVelocityPlannerNode::onLoadPlugin(
-  const LoadPlugin::Request::SharedPtr request,
-  [[maybe_unused]] const LoadPlugin::Response::SharedPtr response)
+  const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request,
+  [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response)
 {
   std::unique_lock<std::mutex> lk(mutex_);
-  planner_manager_.launchScenePlugin(*this, request->plugin_name);
+  planner_manager_.launchScenePlugin(*this, request->data);
 }
 
 void BehaviorVelocityPlannerNode::onUnloadPlugin(
-  const UnloadPlugin::Request::SharedPtr request,
-  [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response)
+  const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request,
+  [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response)
 {
   std::unique_lock<std::mutex> lk(mutex_);
-  planner_manager_.removeScenePlugin(*this, request->plugin_name);
+  planner_manager_.removeScenePlugin(*this, request->data);
 }
 
 void BehaviorVelocityPlannerNode::onParam()

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>