feat(localization): add lidar_marker_localizer

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -18,6 +18,11 @@
   <arg name="eagleye_param_path"/>
   <arg name="ar_tag_based_localizer_param_path"/>
 
+  <!-- parameter paths for lidar_marker_localizer -->
+  <arg name="lidar_marker_localizer/lidar_marker_localizer_param_path"/>
+  <arg name="lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"/>
+  <arg name="lidar_marker_localizer/pointcloud_preprocessor/ring_filter_param_path"/>
+
   <arg name="input_pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
   <arg name="localization_pointcloud_container_name" default="/pointcloud_container"/>
 

launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- input pointcloud topic/container -->
+  <arg name="input_pointcloud"/>
+  <arg name="lidar_container_name"/>
+
+  <!-- whether use intra-process -->
+  <arg name="use_intra_process" default="true" description="use ROS 2 component container communication"/>
+
+  <group>
+    <push-ros-namespace namespace="lidar_marker_localizer"/>
+
+    <!-- pointcloud preprocess -->
+    <group>
+      <push-ros-namespace namespace="pointcloud_preprocessor"/>
+      <load_composable_node target="$(var lidar_container_name)">
+        <composable_node pkg="autoware_pointcloud_preprocessor" plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
+          <remap from="input" to="$(var input_pointcloud)"/>
+          <remap from="output" to="measurement_range/pointcloud"/>
+          <param from="$(var lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path)"/>
+          <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+        </composable_node>
+
+        <composable_node pkg="autoware_pointcloud_preprocessor" plugin="autoware::pointcloud_preprocessor::PassThroughFilterUInt16Component" name="ring_filter">
+          <remap from="input" to="measurement_range/pointcloud"/>
+          <remap from="output" to="ring_filter/pointcloud"/>
+          <param from="$(var lidar_marker_localizer/pointcloud_preprocessor/ring_filter_param_path)"/>
+          <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+        </composable_node>
+      </load_composable_node>
+    </group>
+
+    <!-- lidar_marker_localizer -->
+    <include file="$(find-pkg-share autoware_lidar_marker_localizer)/launch/lidar_marker_localizer.launch.xml">
+      <arg name="input_lanelet2_map" value="/map/vector_map"/>
+      <arg name="input_ekf_pose" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
+      <arg name="input_pointcloud" value="pointcloud_preprocessor/ring_filter/pointcloud"/>
+      <arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="service_trigger_node_srv" value="trigger_node_srv"/>
+      <arg name="param_file" value="$(var lidar_marker_localizer/lidar_marker_localizer_param_path)"/>
+    </include>
+  </group>
+</launch>

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -9,7 +9,7 @@
   <arg name="gnss_enabled" default="true" description="gnss availability for initial position estimation"/>
 
   <!-- split string with underscores -->
-  <let name="available_args" value="[\'ndt\',\'yabloc\',\'eagleye\',\'artag\']"/>
+  <let name="available_args" value="[\'ndt\',\'yabloc\',\'eagleye\',\'artag\',\'lidar-marker\']"/>
   <let name="split_function" value="list(set('$(var pose_source)'.split('_')).intersection($(var available_args)))"/>
   <let name="pose_sources" value="$(eval $(var split_function))"/>
   <let name="multi_localizer_mode" value="$(eval &quot;len($(var pose_sources))> 1&quot;)"/>
@@ -18,6 +18,7 @@
   <let name="use_ndt_pose" value="$(eval &quot;'ndt' in $(var pose_sources)&quot;)"/>
   <let name="use_yabloc_pose" value="$(eval &quot;'yabloc' in $(var pose_sources)&quot;)"/>
   <let name="use_artag_pose" value="$(eval &quot;'artag' in $(var pose_sources)&quot;)"/>
+  <let name="use_lidar_marker_pose" value="$(eval &quot;'lidar-marker' in $(var pose_sources)&quot;)"/>
   <let name="use_eagleye_pose" value="$(eval &quot;'eagleye' in $(var pose_sources)&quot;)"/>
   <let name="use_eagleye_twist" value="$(eval &quot;'eagleye' == '$(var twist_source)'&quot;)"/>
   <let name="use_gyro_odom_twist" value="$(eval &quot;'gyro_odom' == '$(var twist_source)'&quot;)"/>
@@ -91,6 +92,16 @@
     </include>
   </group>
 
+  <!-- LiDAR Marker Localizer (as pose estimator) -->
+  <group if="$(var use_lidar_marker_pose)">
+    <push-ros-namespace namespace="pose_estimator"/>
+    <!-- TODO multi_localizer_mode -->
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml">
+      <arg name="lidar_container_name" value="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container"/>
+      <arg name="input_pointcloud" value="/sensing/lidar/top/pointcloud"/>
+    </include>
+  </group>
+
   <!-- Pose Estimator Arbiter Launch -->
   <group if="$(var multi_localizer_mode)">
     <include file="$(find-pkg-share pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">

launch/tier4_localization_launch/package.xml

@@ -21,6 +21,7 @@
   <exec_depend>autoware_ar_tag_based_localizer</exec_depend>
   <exec_depend>autoware_geo_pose_projector</exec_depend>
   <exec_depend>autoware_gyro_odometer</exec_depend>
+  <exec_depend>autoware_lidar_marker_localizer</exec_depend>
   <exec_depend>autoware_pointcloud_preprocessor</exec_depend>
   <exec_depend>eagleye_geo_pose_fusion</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_lidar_marker_localizer)
+
+# find dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(PCL REQUIRED COMPONENTS common io)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/lidar_marker_localizer.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::lidar_marker_localizer::LidarMarkerLocalizer"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md

@@ -0,0 +1,114 @@
+# LiDAR Marker Localizer
+
+**LiDARMarkerLocalizer** is a detect-reflector-based localization node .
+
+## Inputs / Outputs
+
+### `lidar_marker_localizer` node
+
+#### Input
+
+| Name                   | Type                                            | Description      |
+| :--------------------- | :---------------------------------------------- | :--------------- |
+| `~/input/lanelet2_map` | `autoware_map_msgs::msg::HADMapBin`             | Data of lanelet2 |
+| `~/input/pointcloud`   | `sensor_msgs::msg::PointCloud2`                 | PointCloud       |
+| `~/input/ekf_pose`     | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose         |
+
+#### Output
+
+| Name                            | Type                                            | Description                                                        |
+| :------------------------------ | :---------------------------------------------- | :----------------------------------------------------------------- |
+| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose                                                     |
+| `~/debug/pose_with_covariance`  | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] Estimated pose                                       |
+| `~/debug/marker_detected`       | `geometry_msgs::msg::PoseArray`                 | [debug topic] Detected marker poses                                |
+| `~/debug/marker_mapped`         | `visualization_msgs::msg::MarkerArray`          | [debug topic] Loaded landmarks to visualize in Rviz as thin boards |
+| `~/debug/marker_pointcloud`     | `sensor_msgs::msg::PointCloud2`                 | [debug topic] PointCloud of the detected marker                    |
+| `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs                                                |
+
+## Parameters
+
+{{ json_to_markdown("localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json") }}
+
+## How to launch
+
+When launching Autoware, set `lidar-marker` for `pose_source`.
+
+```bash
+ros2 launch autoware_launch ... \
+    pose_source:=lidar-marker \
+    ...
+```
+
+## Design
+
+### Flowchart
+
+```plantuml
+@startuml
+
+group main process
+  start
+  if (Receive a map?) then (yes)
+  else (no)
+    stop
+  endif
+
+  :Interpolate based on the received ego-vehicle's positions to align with sensor time;
+
+  if (Could interpolate?) then (yes)
+  else (no)
+    stop
+  endif
+
+  :Detect markers (see "Detection Algorithm");
+
+  :Calculate the distance from the ego-vehicle's positions to the nearest marker's position on the lanelet2 map;
+
+  if (Find markers?) then (yes)
+  else (no)
+    if (the distance is nearby?) then (yes)
+      stop
+      note : Error. It should have been able to detect marker
+    else (no)
+      stop
+      note : Not Error. There are no markers around the ego-vehicle
+    endif
+  endif
+
+  :Calculate the correction amount from the ego-vehicle's position;
+
+  if (Is the found marker's position close to the one on the lanelet2 map?) then (yes)
+  else (no)
+    stop
+    note : Detected something that isn't a marker
+  endif
+
+  :Publish result;
+
+  stop
+end group
+
+@enduml
+
+```
+
+## Detection Algorithm
+
+![detection_algorithm](./doc_image/detection_algorithm.png)
+
+1. Split the LiDAR point cloud into rings along the x-axis of the base_link coordinate system at intervals of the `resolution` size.
+2. Find the portion of intensity that matches the `intensity_pattern`.
+3. Perform steps 1 and 2 for each ring, accumulate the matching indices, and detect portions where the count exceeds the `vote_threshold_for_detect_marker` as markers.
+
+## Sample Dataset
+
+- [Sample rosbag and map](https://drive.google.com/file/d/1FuGKbkWrvL_iKmtb45PO9SZl1vAaJFVG/view?usp=sharing)
+
+This dataset was acquired in National Institute for Land and Infrastructure Management, Full-scale tunnel experiment facility.
+The reflectors were installed by [Taisei Corporation](https://www.taisei.co.jp/english/).
+
+## Collaborators
+
+- [TIER IV](https://tier4.jp/en/)
+- [Taisei Corporation](https://www.taisei.co.jp/english/)
+- [Yuri Shimizu](https://github.com/YuriShimizu824)

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml

@@ -0,0 +1,42 @@
+/**:
+  ros__parameters:
+
+    # marker name
+    marker_name: "reflector"
+
+    # for marker detection algorithm
+    resolution: 0.05
+    # A sequence of high/low intensity to perform pattern matching.
+    # 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match)
+    intensity_pattern: [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1]
+    match_intensity_difference_threshold: 20
+    positive_match_num_threshold: 3
+    negative_match_num_threshold: 3
+    vote_threshold_for_detect_marker: 20
+    marker_height_from_ground: 1.075
+
+    # for interpolate algorithm
+    self_pose_timeout_sec: 1.0
+    self_pose_distance_tolerance_m: 1.0
+
+    # for validation
+    limit_distance_from_self_pose_to_nearest_marker: 2.0
+    limit_distance_from_self_pose_to_marker: 2.0
+
+    # base_covariance
+    # [TBD] This value is dynamically scaled according to the distance at which markers are detected.
+    base_covariance: [0.04, 0.0,  0.0,  0.0,        0.0,        0.0,
+                      0.0,  0.04, 0.0,  0.0,        0.0,        0.0,
+                      0.0,  0.0,  0.01, 0.0,        0.0,        0.0,
+                      0.0,  0.0,  0.0,  0.00007569, 0.0,        0.0,
+                      0.0,  0.0,  0.0,  0.0,        0.00007569, 0.0,
+                      0.0,  0.0,  0.0,  0.0,        0.0,        0.00030625]
+
+    # for visualize the detected marker pointcloud
+    marker_width: 0.8
+
+    # for save log
+    enable_save_log: false
+    save_file_directory_path: detected_reflector_intensity
+    save_file_name: detected_reflector_intensity
+    save_frame_id: velodyne_top

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml

@@ -0,0 +1,24 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share lidar_marker_localizer)/config/lidar_marker_localizer.param.yaml"/>
+
+  <!-- Topic names -->
+  <arg name="input_lanelet2_map" default="~/input/lanelet2_map"/>
+  <arg name="input_ekf_pose" default="~/input/ekf_pose"/>
+  <arg name="input_pointcloud" default="~/input/pointcloud"/>
+
+  <arg name="output_pose_with_covariance" default="~/output/pose_with_covariance"/>
+
+  <arg name="service_trigger_node_srv" default="~/service/trigger_node_srv"/>
+
+  <node pkg="autoware_lidar_marker_localizer" exec="autoware_lidar_marker_localizer_node" output="both">
+    <remap from="~/input/pointcloud" to="$(var input_pointcloud)"/>
+    <remap from="~/input/ekf_pose" to="$(var input_ekf_pose)"/>
+    <remap from="~/input/lanelet2_map" to="$(var input_lanelet2_map)"/>
+
+    <remap from="~/output/pose_with_covariance" to="$(var output_pose_with_covariance)"/>
+
+    <remap from="~/service/trigger_node_srv" to="$(var service_trigger_node_srv)"/>
+
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml

@@ -0,0 +1,40 @@
+<?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_lidar_marker_localizer</name>
+  <version>0.0.0</version>
+  <description>The autoware_lidar_marker_localizer package</description>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Eijiro Takeuchi</author>
+  <author email="[email protected]">Yamato Ando</author>
+  <author email="[email protected]">Shintaro Sakoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_landmark_manager</depend>
+  <depend>autoware_map_msgs</depend>
+  <depend>autoware_point_types</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>localization_util</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>