feat(localization): add lidar_marker_localizer (#861)

* add config files

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

* add param marker_height_from_ground

Signed-off-by: Yamato Ando <[email protected]>

* save log param

Signed-off-by: Yamato Ando <[email protected]>

* apply PointXYZIRC

Signed-off-by: Yamato Ando <[email protected]>

* to pass spell-check

Signed-off-by: Yamato Ando <[email protected]>

* refactor

Signed-off-by: Yamato Ando <[email protected]>

* change flag

Signed-off-by: Yamato Ando <[email protected]>

* fix typo

Signed-off-by: Yamato Ando <[email protected]>

---------

Signed-off-by: Yamato Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: SakodaShintaro <[email protected]>

autoware_launch/config/localization/lidar_marker_localizer/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

autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    input_frame: "base_link"
+    output_frame: "base_link"
+    min_x: -10.0
+    max_x: 10.0
+    min_y: 0.0
+    max_y: 7.5
+    min_z: -5.0
+    max_z: 5.0
+    negative: False

autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/ring_filter.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    input_frame: "base_link"
+    output_frame: "base_link"
+    filter_field_name: "channel"
+    filter_limit_min: 5
+    filter_limit_max: 45
+    filter_limit_negative: False
+    keep_organized: False

autoware_launch/launch/components/tier4_localization_component.launch.xml

@@ -51,6 +51,14 @@
       <!-- parameter paths for eagleye -->
       <arg name="eagleye_param_path" value="$(find-pkg-share autoware_launch)/config/localization/eagleye_config.param.yaml"/>
 
+      <!-- parameter paths for lidar_marker_localizer -->
+      <arg name="lidar_marker_localizer/lidar_marker_localizer_param_path" value="$(var loc_config_path)/lidar_marker_localizer/lidar_marker_localizer.param.yaml"/>
+      <arg
+        name="lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"
+        value="$(var loc_config_path)/lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml"
+      />
+      <arg name="lidar_marker_localizer/pointcloud_preprocessor/ring_filter_param_path" value="$(var loc_config_path)/lidar_marker_localizer/pointcloud_preprocessor/ring_filter.param.yaml"/>
+
       <!-- parameter path for ar_tag_based_localizer -->
       <arg name="ar_tag_based_localizer_param_path" value="$(var loc_config_path)/ar_tag_based_localizer.param.yaml"/>
     </include>