test(blind_spot): add unit tests

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

Author: soblin

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt

@@ -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") # // cspell: ignore DEXPORT
+  endif()
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  # NOTE(soblin): pybind11::scoped_interpreter needs to be initialized globally, 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/include/autoware/behavior_velocity_blind_spot_module/manager.hpp

@@ -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/parameter.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/scene.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/util.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_
 
 #include <autoware/route_handler/route_handler.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
 
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
@@ -25,6 +26,7 @@
 #include <memory>
 #include <optional>
 #include <utility>
+#include <vector>
 
 namespace autoware::behavior_velocity_planner
 {
@@ -50,31 +52,49 @@ std::optional<InterpolatedPathInfo> generateInterpolatedPathInfo(
   const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path,
   rclcpp::Logger logger);
 
+std::optional<size_t> getFirstPointIntersectsLineByFootprint(
+  const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info,
+  const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length);
+
 std::optional<lanelet::ConstLanelet> getSiblingStraightLanelet(
   const lanelet::Lanelet assigned_lane,
   const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr);
 
 /**
- * @brief Create half lanelet
- * @param lanelet input lanelet
- * @return Half lanelet
+ * @brief generate a new lanelet object on the `turn_direction` side of `lanelet` which is offset
+ * from `ignore_width_from_centerline` from the centerline of `lanelet`
+ * @return new lanelet object
  */
 lanelet::ConstLanelet generateHalfLanelet(
   const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction,
   const double ignore_width_from_centerline);
 
+/**
+ * @brief generate a new lanelet object from the `turn_direction` side neighbouring lanelet of the
+ * input `lanelet` by the width of `adjacent_extend_width`
+ * @param new lanelet object
+ */
 lanelet::ConstLanelet generateExtendedAdjacentLanelet(
   const lanelet::ConstLanelet lanelet, const TurnDirection direction,
   const double adjacent_extend_width);
+
+/**
+ * @brief generate a new lanelet object from the `turn_direction` side neighbouring opposite lanelet
+ * of the input `lanelet` by the width of `opposite_adjacent_extend_width`
+ * @param new lanelet object
+ */
 lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet(
   const lanelet::ConstLanelet lanelet, const TurnDirection direction,
   const double opposite_adjacent_extend_width);
 
+std::vector<lanelet::Id> find_lane_ids_upto(
+  const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id);
+
 lanelet::ConstLanelets generateBlindSpotLanelets(
   const std::shared_ptr<autoware::route_handler::RouteHandler> route_handler,
-  const TurnDirection turn_direction, const lanelet::Id lane_id,
-  const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline,
-  const double adjacent_extend_width, const double opposite_adjacent_extend_width);
+  const TurnDirection turn_direction, const std::vector<lanelet::Id> lane_ids_upto_intersection,
+  const double ignore_width_from_centerline, const double adjacent_extend_width,
+  const double opposite_adjacent_extend_width);
 
 /**
  * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index.

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml

@@ -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/src/manager.cpp

@@ -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/parameter.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/scene.cpp

@@ -101,8 +101,9 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * pat
   }
 
   if (!blind_spot_lanelets_) {
+    const auto lane_ids_upto_intersection = find_lane_ids_upto(input_path, lane_id_);
     const auto blind_spot_lanelets = generateBlindSpotLanelets(
-      planner_data_->route_handler_, turn_direction_, lane_id_, input_path,
+      planner_data_->route_handler_, turn_direction_, lane_ids_upto_intersection,
       planner_param_.ignore_width_from_center_line, planner_param_.adjacent_extend_width,
       planner_param_.opposite_adjacent_extend_width);
     if (!blind_spot_lanelets.empty()) {
@@ -179,27 +180,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path)
   return true;
 }
 
-static std::optional<size_t> getFirstPointIntersectsLineByFootprint(
-  const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info,
-  const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length)
-{
-  const auto & path_ip = interpolated_path_info.path;
-  const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value();
-  const size_t vehicle_length_idx = static_cast<size_t>(vehicle_length / interpolated_path_info.ds);
-  const size_t start =
-    static_cast<size_t>(std::max<int>(0, static_cast<int>(lane_start) - vehicle_length_idx));
-  const auto line2d = line.basicLineString();
-  for (auto i = start; i <= lane_end; ++i) {
-    const auto & base_pose = path_ip.points.at(i).point.pose;
-    const auto path_footprint = autoware::universe_utils::transformVector(
-      footprint, autoware::universe_utils::pose2transform(base_pose));
-    if (bg::intersects(path_footprint, line2d)) {
-      return std::make_optional<size_t>(i);
-    }
-  }
-  return std::nullopt;
-}
-
 static std::optional<size_t> getDuplicatedPointIdx(
   const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point)
 {

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp

@@ -88,6 +88,27 @@ std::optional<InterpolatedPathInfo> generateInterpolatedPathInfo(
   return interpolated_path_info;
 }
 
+std::optional<size_t> getFirstPointIntersectsLineByFootprint(
+  const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info,
+  const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length)
+{
+  const auto & path_ip = interpolated_path_info.path;
+  const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value();
+  const size_t vehicle_length_idx = static_cast<size_t>(vehicle_length / interpolated_path_info.ds);
+  const size_t start =
+    static_cast<size_t>(std::max<int>(0, static_cast<int>(lane_start) - vehicle_length_idx));
+  const auto line2d = line.basicLineString();
+  for (auto i = start; i <= lane_end; ++i) {
+    const auto & base_pose = path_ip.points.at(i).point.pose;
+    const auto path_footprint = autoware::universe_utils::transformVector(
+      footprint, autoware::universe_utils::pose2transform(base_pose));
+    if (boost::geometry::intersects(path_footprint, line2d)) {
+      return std::make_optional<size_t>(i);
+    }
+  }
+  return std::nullopt;
+}
+
 std::optional<lanelet::ConstLanelet> getSiblingStraightLanelet(
   const lanelet::Lanelet assigned_lane,
   const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr)
@@ -200,35 +221,39 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line)
   return lanelet::LineString3d(lanelet::InvalId, pts);
 }
 
-lanelet::ConstLanelets generateBlindSpotLanelets(
-  const std::shared_ptr<autoware::route_handler::RouteHandler> route_handler,
-  const TurnDirection turn_direction, const lanelet::Id lane_id,
-  const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline,
-  const double adjacent_extend_width, const double opposite_adjacent_extend_width)
+std::vector<lanelet::Id> find_lane_ids_upto(
+  const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id)
 {
-  const auto lanelet_map_ptr = route_handler->getLaneletMapPtr();
-  const auto routing_graph_ptr = route_handler->getRoutingGraphPtr();
-
   std::vector<int64_t> lane_ids;
   /* get lane ids until intersection */
   for (const auto & point : path.points) {
     bool found_intersection_lane = false;
     for (const auto id : point.lane_ids) {
       if (id == lane_id) {
         found_intersection_lane = true;
-        lane_ids.push_back(lane_id);
         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;
   }
+  return lane_ids;
+}
+
+lanelet::ConstLanelets generateBlindSpotLanelets(
+  const std::shared_ptr<autoware::route_handler::RouteHandler> route_handler,
+  const TurnDirection turn_direction, const std::vector<lanelet::Id> lane_ids_upto_intersection,
+  const double ignore_width_from_centerline, const double adjacent_extend_width,
+  const double opposite_adjacent_extend_width)
+{
+  const auto lanelet_map_ptr = route_handler->getLaneletMapPtr();
+  const auto routing_graph_ptr = route_handler->getRoutingGraphPtr();
 
   lanelet::ConstLanelets blind_spot_lanelets;
-  for (const auto i : lane_ids) {
+  for (const auto i : lane_ids_upto_intersection) {
     const auto lane = lanelet_map_ptr->laneletLayer.get(i);
     const auto ego_half_lanelet =
       generateHalfLanelet(lane, turn_direction, ignore_width_from_centerline);