test(blind_spot): add unit tests

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

common/autoware_test_utils/test_map/intersection/lanelet2_map.osm

@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.14)
 project(autoware_behavior_velocity_blind_spot_module)
 
+option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" OFF)
+
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml)
@@ -11,6 +13,20 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/scene.cpp
   src/decisions.cpp
   src/util.cpp
+  src/parameter.cpp
 )
 
-ament_auto_package(INSTALL_TO_SHARE config)
+if(BUILD_TESTING)
+  if(EXPORT_TEST_PLOT_FIGURE)
+    add_definitions(-DEXPORT_TEST_PLOT_FIGURE "-Wno-attributes")
+  endif()
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  # NOTE(soblin): pybind11::scoped_interpreter needs to be initialized globaly, not in the FixtureClass instantiated for each test suite
+  ament_add_gtest(test_${PROJECT_NAME}_util
+    test/test_util.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}_util ${PROJECT_NAME})
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE config test_data)

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_
 #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_
 
+#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp"
 #include "autoware/behavior_velocity_blind_spot_module/scene.hpp"
 
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
@@ -38,7 +39,7 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC
   const char * getModuleName() override { return "blind_spot"; }
 
 private:
-  BlindSpotModule::PlannerParam planner_param_;
+  PlannerParam planner_param_;
 
   void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp

@@ -0,0 +1,40 @@
+// 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_BLIND_SPOT_MODULE__PARAMETER_HPP_
+#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_
+
+#include <rclcpp/node.hpp>
+
+#include <string>
+
+namespace autoware::behavior_velocity_planner
+{
+struct PlannerParam
+{
+  static PlannerParam init(rclcpp::Node & node, const std::string & ns);
+  bool use_pass_judge_line{};
+  double stop_line_margin{};
+  double backward_detection_length{};
+  double ignore_width_from_center_line{};
+  double adjacent_extend_width{};
+  double opposite_adjacent_extend_width{};
+  double max_future_movement_time{};
+  double ttc_min{};
+  double ttc_max{};
+  double ttc_ego_minimal_velocity{};
+};
+}  // namespace autoware::behavior_velocity_planner
+
+#endif  // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_
 #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_
 
+#include <autoware/behavior_velocity_blind_spot_module/parameter.hpp>
 #include <autoware/behavior_velocity_blind_spot_module/util.hpp>
 #include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/utilization/state_machine.hpp>
@@ -70,20 +71,6 @@ class BlindSpotModule : public SceneModuleInterface
   };
 
 public:
-  struct PlannerParam
-  {
-    bool use_pass_judge_line;
-    double stop_line_margin;
-    double backward_detection_length;
-    double ignore_width_from_center_line;
-    double adjacent_extend_width;
-    double opposite_adjacent_extend_width;
-    double max_future_movement_time;
-    double ttc_min;
-    double ttc_max;
-    double ttc_ego_minimal_velocity;
-  };
-
   BlindSpotModule(
     const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
     const std::shared_ptr<const PlannerData> planner_data, const PlannerParam & planner_param,

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp

@@ -23,6 +23,7 @@
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_route_handler</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>geometry_msgs</depend>
   <depend>pluginlib</depend>
@@ -31,6 +32,7 @@
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml

@@ -34,23 +34,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
     node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc"))
 {
   const std::string ns(BlindSpotModuleManager::getModuleName());
-  planner_param_.use_pass_judge_line =
-    getOrDeclareParameter<bool>(node, ns + ".use_pass_judge_line");
-  planner_param_.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
-  planner_param_.backward_detection_length =
-    getOrDeclareParameter<double>(node, ns + ".backward_detection_length");
-  planner_param_.ignore_width_from_center_line =
-    getOrDeclareParameter<double>(node, ns + ".ignore_width_from_center_line");
-  planner_param_.adjacent_extend_width =
-    getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
-  planner_param_.opposite_adjacent_extend_width =
-    getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
-  planner_param_.max_future_movement_time =
-    getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
-  planner_param_.ttc_min = getOrDeclareParameter<double>(node, ns + ".ttc_min");
-  planner_param_.ttc_max = getOrDeclareParameter<double>(node, ns + ".ttc_max");
-  planner_param_.ttc_ego_minimal_velocity =
-    getOrDeclareParameter<double>(node, ns + ".ttc_ego_minimal_velocity");
+  planner_param_ = PlannerParam::init(node, ns);
 }
 
 void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path)

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp

@@ -0,0 +1,45 @@
+// 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_blind_spot_module/parameter.hpp"
+
+#include <autoware/universe_utils/ros/parameter.hpp>
+
+#include <string>
+
+namespace autoware::behavior_velocity_planner
+{
+
+PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns)
+{
+  using autoware::universe_utils::getOrDeclareParameter;
+  PlannerParam param;
+  param.use_pass_judge_line = getOrDeclareParameter<bool>(node, ns + ".use_pass_judge_line");
+  param.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
+  param.backward_detection_length =
+    getOrDeclareParameter<double>(node, ns + ".backward_detection_length");
+  param.ignore_width_from_center_line =
+    getOrDeclareParameter<double>(node, ns + ".ignore_width_from_center_line");
+  param.adjacent_extend_width = getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
+  param.opposite_adjacent_extend_width =
+    getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
+  param.max_future_movement_time =
+    getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
+  param.ttc_min = getOrDeclareParameter<double>(node, ns + ".ttc_min");
+  param.ttc_max = getOrDeclareParameter<double>(node, ns + ".ttc_max");
+  param.ttc_ego_minimal_velocity =
+    getOrDeclareParameter<double>(node, ns + ".ttc_ego_minimal_velocity");
+  return param;
+}
+}  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp

@@ -220,8 +220,8 @@ lanelet::ConstLanelets generateBlindSpotLanelets(
         break;
       }
       // make lane_ids unique
-      if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) {
-        lane_ids.push_back(lane_id);
+      if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) {
+        lane_ids.push_back(id);
       }
     }
     if (found_intersection_lane) break;
@@ -301,6 +301,7 @@ lanelet::ConstLanelets generateBlindSpotLanelets(
       blind_spot_lanelets.push_back(ego_half_lanelet);
     }
   }
+
   return blind_spot_lanelets;
 }