fix(lidar_marker_localizer): unintended implementation fixes

autoware_launch/config/localization/lidar_marker_localizer/lidar_marker_localizer.param.yaml → autoware_launch/config/localization/lidar_marker_localizer/top_left/lidar_marker_localizer.param.yaml

@@ -1,10 +1,18 @@
 /**:
   ros__parameters:
 
+    enable_read_all_target_ids: true
+    target_ids: [
+                 "01", "02", "03", "04", "05", "06", "07", "08", "09", "10",
+                 "11", "12", "13", "14", "15", "16", "17", "18", "19"
+                ]
+    queue_size_for_output_pose: 10 # Queue size for output/pose_with_covariance publisher
+
     # marker name
     marker_name: "reflector"
 
     # for marker detection algorithm
+    road_surface_mode: false
     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)
@@ -13,14 +21,19 @@
     positive_match_num_threshold: 3
     negative_match_num_threshold: 3
     vote_threshold_for_detect_marker: 20
+    marker_to_vehicle_offset_y: 2.3
     marker_height_from_ground: 1.075
+    lower_ring_id_init: 128
+    upper_ring_id_init: 0
+    reference_ring_number: 255 # set to 255 to disable ring filtering
 
     # 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_nearest_marker_y: 1.0
     limit_distance_from_self_pose_to_marker: 2.0
 
     # base_covariance
@@ -32,11 +45,11 @@
                       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_directory_path: detected_reflector_intensity_top_left
     save_file_name: detected_reflector_intensity
     save_frame_id: velodyne_top
+    # for visualize the detected marker pointcloud
+    radius_for_extracting_marker_pointcloud: 0.4 # [m] within
+    queue_size_for_debug_pub_msg: 10 # Queue size for debug publishers

autoware_launch/config/localization/lidar_marker_localizer/top_right/lidar_marker_localizer.param.yaml

@@ -0,0 +1,55 @@
+/**:
+  ros__parameters:
+
+    enable_read_all_target_ids: true
+    target_ids: [
+                 "01", "02", "03", "04", "05", "06", "07", "08", "09", "10",
+                 "11", "12", "13", "14", "15", "16", "17", "18", "19"
+                ]
+    queue_size_for_output_pose: 10 # Queue size for output/pose_with_covariance publisher
+
+    # marker name
+    marker_name: "reflector"
+
+    # for marker detection algorithm
+    road_surface_mode: false
+    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_to_vehicle_offset_y: -2.3
+    marker_height_from_ground: 1.075
+    lower_ring_id_init: 128
+    upper_ring_id_init: 0
+    reference_ring_number: 255 # set to 255 to disable ring filtering
+
+    # 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_nearest_marker_y: 1.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 save log
+    enable_save_log: false
+    save_file_directory_path: detected_reflector_intensity_top_right
+    save_file_name: detected_reflector_intensity
+    save_frame_id: velodyne_top
+    # for visualize the detected marker pointcloud
+    radius_for_extracting_marker_pointcloud: 0.4 # [m] within
+    queue_size_for_debug_pub_msg: 10 # Queue size for debug publishers

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

@@ -0,0 +1,12 @@
+/**:
+  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
+    processing_time_threshold_sec: 0.01

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

@@ -78,12 +78,25 @@
       <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/top_left/lidar_marker_localizer_param_path" value="$(var loc_config_path)/lidar_marker_localizer/top_left/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"
+        name="lidar_marker_localizer/top_left/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"
+        value="$(var loc_config_path)/lidar_marker_localizer/top_left/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml"
+      />
+      <arg
+        name="lidar_marker_localizer/top_left/pointcloud_preprocessor/ring_filter_param_path"
+        value="$(var loc_config_path)/lidar_marker_localizer/top_left/pointcloud_preprocessor/ring_filter.param.yaml"
+      />
+
+      <arg name="lidar_marker_localizer/top_right/lidar_marker_localizer_param_path" value="$(var loc_config_path)/lidar_marker_localizer/top_right/lidar_marker_localizer.param.yaml"/>
+      <arg
+        name="lidar_marker_localizer/top_right/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"
+        value="$(var loc_config_path)/lidar_marker_localizer/top_right/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml"
+      />
+      <arg
+        name="lidar_marker_localizer/top_right/pointcloud_preprocessor/ring_filter_param_path"
+        value="$(var loc_config_path)/lidar_marker_localizer/top_right/pointcloud_preprocessor/ring_filter.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"/>

tier4_universe_launch/tier4_localization_launch/launch/localization.launch.xml

@@ -20,13 +20,14 @@
   <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"/>
+  <!-- parameter paths for lidar_marker_localizer instances -->
+  <arg name="lidar_marker_localizer/top_left/lidar_marker_localizer_param_path"/>
+  <arg name="lidar_marker_localizer/top_left/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"/>
+  <arg name="lidar_marker_localizer/top_left/pointcloud_preprocessor/ring_filter_param_path"/>
 
-  <arg name="input_pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
-  <arg name="localization_pointcloud_container_name" default="/pointcloud_container"/>
+  <arg name="lidar_marker_localizer/top_right/lidar_marker_localizer_param_path"/>
+  <arg name="lidar_marker_localizer/top_right/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"/>
+  <arg name="lidar_marker_localizer/top_right/pointcloud_preprocessor/ring_filter_param_path"/>
 
   <!-- localization module -->
   <group>

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

@@ -1,30 +1,36 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="group_namespace" default="lidar_marker_localizer"/>
+
   <!-- input pointcloud topic/container -->
   <arg name="input_pointcloud"/>
-  <arg name="lidar_container_name"/>
+  <arg name="lidar_container_name" default="/pointcloud_preprocessor/pointcloud_container" description="container name of lidar used for lidar-marker localization"/>
 
   <!-- whether use intra-process -->
   <arg name="use_intra_process" default="true" description="use ROS 2 component container communication"/>
 
+  <arg name="crop_box_filter_measurement_range_param_path" default="$(var lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path)"/>
+  <arg name="ring_filter_param_path" default="$(var lidar_marker_localizer/pointcloud_preprocessor/ring_filter_param_path)"/>
+  <arg name="lidar_marker_localizer_param_path" default="$(var lidar_marker_localizer/lidar_marker_localizer_param_path)"/>
+
   <group>
-    <push-ros-namespace namespace="lidar_marker_localizer"/>
+    <push-ros-namespace namespace="$(var group_namespace)"/>
 
     <!-- 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">
+          <param from="$(var crop_box_filter_measurement_range_param_path)"/>
           <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">
+          <param from="$(var ring_filter_param_path)"/>
           <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>
@@ -37,7 +43,7 @@
       <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)"/>
+      <arg name="param_file" value="$(var lidar_marker_localizer_param_path)"/>
     </include>
   </group>
 </launch>

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

@@ -96,10 +96,28 @@
   <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>
+    <!-- modify lidar_marker_localizer group values following the lidar system that uses lidar_marker_localizer -->
+    <group>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml">
+        <arg name="group_namespace" value="lidar_marker_localizer/top_left"/>
+        <arg name="lidar_container_name" value="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container"/>
+        <arg name="input_pointcloud" value="/sensing/lidar/top/pointcloud_raw_ex"/>
+        <arg name="crop_box_filter_measurement_range_param_path" value="$(var lidar_marker_localizer/top_left/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path)"/>
+        <arg name="ring_filter_param_path" value="$(var lidar_marker_localizer/top_left/pointcloud_preprocessor/ring_filter_param_path)"/>
+        <arg name="lidar_marker_localizer_param_path" value="$(var lidar_marker_localizer/top_left/lidar_marker_localizer_param_path)"/>
+      </include>
+    </group>
+
+    <group>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml">
+        <arg name="group_namespace" value="lidar_marker_localizer/top_right"/>
+        <arg name="lidar_container_name" value="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container"/>
+        <arg name="input_pointcloud" value="/sensing/lidar/top/pointcloud_raw_ex"/>
+        <arg name="crop_box_filter_measurement_range_param_path" value="$(var lidar_marker_localizer/top_right/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path)"/>
+        <arg name="ring_filter_param_path" value="$(var lidar_marker_localizer/top_right/pointcloud_preprocessor/ring_filter_param_path)"/>
+        <arg name="lidar_marker_localizer_param_path" value="$(var lidar_marker_localizer/top_right/lidar_marker_localizer_param_path)"/>
+      </include>
+    </group>
   </group>
 
   <!-- Pose Estimator Arbiter Launch -->