feat(bvp): add detection lane module

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -26,6 +26,7 @@
   <arg name="launch_intersection_module" default="true"/>
   <arg name="launch_merge_from_private_module" default="true"/>
   <arg name="launch_blind_spot_module" default="true"/>
+  <arg name="launch_detection_lane_module" default="true"/>
   <arg name="launch_detection_area_module" default="true"/>
   <arg name="launch_virtual_traffic_light_module" default="true"/>
   <arg name="launch_no_stopping_area_module" default="true"/>
@@ -128,6 +129,11 @@
     value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'autoware::behavior_velocity_planner::BlindSpotModulePlugin, '&quot;)"
     if="$(var launch_blind_spot_module)"
   />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'autoware::behavior_velocity_planner::DetectionLaneModulePlugin, '&quot;)"
+    if="$(var launch_detection_lane_module)"
+  />
   <let
     name="behavior_velocity_planner_launch_modules"
     value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'autoware::behavior_velocity_planner::DetectionAreaModulePlugin, '&quot;)"
@@ -170,6 +176,8 @@
   />
   <let name="behavior_velocity_planner_launch_modules" value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'&quot;)"/>
 
+  <arg name="behavior_velocity_planner_detection_lane_module_param_path" default="$(find-pkg-share autoware_behavior_velocity_detection_lane_module)/config/detection_lane.param.yaml"/>
+
   <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="both">
     <composable_node pkg="autoware_glog_component" plugin="autoware::glog_component::GlogComponent" name="glog_component" namespace=""/>
     <composable_node pkg="autoware_behavior_velocity_planner" plugin="autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" name="behavior_velocity_planner" namespace="">
@@ -214,6 +222,7 @@
       <param from="$(var behavior_velocity_planner_run_out_module_param_path)"/>
       <param from="$(var behavior_velocity_planner_speed_bump_module_param_path)"/>
       <param from="$(var behavior_velocity_planner_no_drivable_lane_module_param_path)"/>
+      <param from="$(var behavior_velocity_planner_detection_lane_module_param_path)"/>
       <!-- composable node config -->
       <extra_arg name="use_intra_process_comms" value="false"/>
     </composable_node>

planning/autoware_rtc_interface/src/rtc_interface.cpp

@@ -70,7 +70,7 @@
     module.type = Module::BLIND_SPOT;
   } else if (module_name == "crosswalk") {
     module.type = Module::CROSSWALK;
-  } else if (module_name == "detection_area") {
+  } else if (module_name == "detection_area" || module_name == "detection_lane") {
     module.type = Module::DETECTION_AREA;
   } else if (module_name == "intersection") {
     module.type = Module::INTERSECTION;

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_behavior_velocity_detection_lane_module)
+
+find_package(autoware_cmake REQUIRED)
+
+autoware_package()
+
+find_package(PCL REQUIRED COMPONENTS common)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-error=deprecated-declarations)
+endif()
+
+pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml)
+
+generate_parameter_library(detection_lane_parameters
+  param/detection_lane_parameters.yaml
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${PCL_LIBRARIES}
+  detection_lane_parameters
+)
+
+# if(BUILD_TESTING)
+#   find_package(ament_lint_auto REQUIRED)
+#   ament_lint_auto_find_test_dependencies()
+#   ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+#     test/test_utils.cpp
+#     test/test_node_interface.cpp
+#   )
+#   target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
+# endif()
+
+ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/README.md

@@ -0,0 +1 @@
+## Detection Lane

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/config/detection_lane.param.yaml

@@ -0,0 +1,58 @@
+/**:
+  ros__parameters:
+    detection_lane:
+      pointcloud:
+        range:
+          min: 80.0
+          max: 140.0
+        offset:
+          left: -0.3
+          right: -0.3
+        voxel_grid_filter:
+          x: 0.1
+          y: 0.1
+          z: 0.5
+        clustering:
+          cluster_tolerance: 0.5  #[m]
+          min_cluster_height: 0.1
+          min_cluster_size: 5
+          max_cluster_size: 10000
+        velocity_estimation:
+          observation_time: 0.3
+        latency: 0.3
+
+      filter:
+        min_velocity: 1.0
+
+      stop:
+        margin: 0.0
+        on_time_buffer: 1.0
+        off_time_buffer: 0.5
+        use_sudden_stop: true
+
+      hold:
+        margin: 0.0
+
+      dead_line:
+        enable: true
+        margin: 0.5
+
+      enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
+
+    intersection_names: [shiojiri, komatsu]
+
+    config:
+      shiojiri:
+        mgrs_grid: 53SQA
+        lane_id: 192
+        stop_line_id: 88642
+        lane_1st_ids: [53437, 53400, 53506]
+        ttc_threshold: 20.0
+
+      komatsu:
+        mgrs_grid: 53SPA
+        lane_id: 185
+        stop_line_id: 128271
+        lane_1st_ids: [128324, 128080, 128176, 128228, 128122, 128074, 128248, 176]
+        lane_2nd_ids: [128323, 128079, 128175, 128227, 128226, 128073, 128247, 175]
+        ttc_threshold: 20.0

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/package.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_behavior_velocity_detection_lane_module</name>
+  <version>0.43.0</version>
+  <description>The autoware_behavior_velocity_detection_lane_module package</description>
+
+  <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
+  <maintainer email="[email protected]">Tomoya Kimura</maintainer>
+  <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Kyoichi Sugahara</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <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_behavior_velocity_rtc_interface</depend>
+  <depend>autoware_lanelet2_extension</depend>
+  <depend>autoware_motion_utils</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_route_handler</depend>
+  <depend>autoware_signal_processing</depend>
+  <depend>autoware_utils</depend>
+  <depend>eigen</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libboost-dev</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_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>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/param/detection_lane_parameters.yaml

@@ -0,0 +1,116 @@
+detection_lane:
+  detection_lane:
+    pointcloud:
+      range:
+        min:
+          type: double
+          validation:
+            gt_eq<>: [0.0]
+        max:
+          type: double
+          validation:
+            gt_eq<>: [0.0]
+      offset:
+        left:
+          type: double
+        right:
+          type: double
+      voxel_grid_filter:
+        x:
+          type: double
+          validation:
+            gt<>: [0.0]
+        y:
+          type: double
+          validation:
+            gt<>: [0.0]
+        z:
+          type: double
+          validation:
+            gt<>: [0.0]
+      clustering:
+        cluster_tolerance:
+          type: double
+          validation:
+            gt<>: [0.0]
+        min_cluster_height:
+          type: double
+          validation:
+            gt<>: [0.0]
+        min_cluster_size:
+          type: int
+          validation:
+            gt<>: [0]
+        max_cluster_size:
+          type: int
+          validation:
+            gt<>: [0]
+      velocity_estimation:
+        observation_time:
+          type: double
+          validation:
+            gt_eq<>: [0.0]
+      latency:
+        type: double
+        validation:
+          gt_eq<>: [0.0]
+
+    filter:
+      min_velocity:
+        type: double
+
+    stop:
+      margin:
+        type: double
+      on_time_buffer:
+        type: double
+      off_time_buffer:
+        type: double
+      use_sudden_stop:
+        type: bool
+
+    hold:
+      margin:
+        type: double
+
+    dead_line:
+      enable:
+        type: bool
+      margin:
+        type: double
+
+  intersection_names:
+    type: string_array
+
+  config:
+    __map_intersection_names:
+      mgrs_grid:
+        type: string
+      lane_id:
+        type: int
+      stop_line_id:
+        type: int
+      lane_1st_ids:
+        type: int_array
+        validation:
+          not_empty<>: []
+          unique<>: []
+      lane_2nd_ids:
+        type: int_array
+        default_value: []
+        validation:
+          unique<>: []
+      lane_3rd_ids:
+        type: int_array
+        default_value: []
+        validation:
+          unique<>: []
+      lane_4th_ids:
+        type: int_array
+        default_value: []
+        validation:
+          unique<>: []
+      ttc_threshold:
+        type: double
+        validation:
+          gt_eq<>: [0.0]

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/plugins.xml

@@ -0,0 +1,3 @@
+<library path="autoware_behavior_velocity_detection_lane_module">
+  <class type="autoware::behavior_velocity_planner::DetectionLaneModulePlugin" base_class_type="autoware::behavior_velocity_planner::PluginInterface"/>
+</library>