for experiment/20240627_lidar_marker_lolicazer

Signed-off-by: Motsu-san <[email protected]>

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -21,6 +21,14 @@
   <arg name="input_pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
   <arg name="localization_pointcloud_container_name" default="/pointcloud_container"/>
 
+  <!-- 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/top/pointcloud"/>
+  <arg name="lidar_container_name" default="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container"/>
+
   <!-- localization module -->
   <group>
     <push-ros-namespace namespace="localization"/>

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="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
+          <remap from="input" to="$(var input_pointcloud)"/>
+          <remap from="output" to="measurement_range/pointcloud_ex"/>
+          <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="pointcloud_preprocessor" plugin="pointcloud_preprocessor::PassThroughFilterUInt16Component" name="ring_filter">
+          <remap from="input" to="measurement_range/pointcloud_ex"/>
+          <remap from="output" to="ring_filter/pointcloud_ex"/>
+          <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 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_ex"/>
+      <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

@@ -32,6 +32,42 @@
     <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
   </group>
 
+
+  <!-- NDT Scan Matcher And LiDAR Marker Localizer Launch (as pose estimator) -->
+  <group if="$(eval &quot;'$(var pose_source)'=='ndt_lidar-marker'&quot;)">
+    <group>
+      <push-ros-namespace namespace="pose_estimator"/>
+      <!-- NDT Scan Matcher -->
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
+
+      <!-- LiDAR Marker Localizer -->
+      <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/rectified/pointcloud_ex"/>
+      </include>
+
+    </group>
+    <group>
+      <push-ros-namespace namespace="util"/>
+      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+        <arg name="ndt_enabled" value="true"/>
+        <arg name="gnss_enabled" value="true"/>
+        <arg name="ekf_enabled" value="true"/>
+        <arg name="yabloc_enabled" value="false"/>
+        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
+        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+        <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
+      </include>
+      <group if="$(var gnss_enabled)">
+        <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
+      </group>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
+        <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
+      </include>
+    </group>
+  </group>
+
+
   <!-- YabLoc Launch (as pose estimator) -->
   <group if="$(var use_yabloc_pose)">
     <push-ros-namespace namespace="pose_estimator"/>

localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp

@@ -50,7 +50,7 @@ void LandmarkManager::parse_landmarks(
     const auto & v2 = vertices[2];
     const auto & v3 = vertices[3];
     const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0;
-    const double volume_threshold = 1e-5;
+    const double volume_threshold = 1e-3;
     if (volume > volume_threshold) {
       continue;
     }

localization/landmark_based_localizer/lidar_marker_localizer/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.14)
+project(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_executable(lidar_marker_localizer
+  src/lidar_marker_localizer.cpp
+  src/diagnostics/diagnostics_module.cpp
+  src/main.cpp
+)
+
+target_link_libraries(lidar_marker_localizer
+  ${PCL_LIBRARIES}
+)
+
+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/landmark_based_localizer/lidar_marker_localizer/README.md

@@ -0,0 +1,113 @@
+# 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_auto_mapping_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 |
+| `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs                                                |
+
+## Parameters
+
+{{ json_to_markdown("localization/landmark_based_localizer/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/landmark_based_localizer/lidar_marker_localizer/config/lidar_marker_localizer.param.yaml

@@ -0,0 +1,39 @@
+/**:
+  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 save log
+    enable_save_log: false
+    savefile_directory_path: detected_reflector_intensity
+    savefile_name: detected_reflector_intensity
+    save_frame_id: velodyne_top

localization/landmark_based_localizer/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="lidar_marker_localizer" exec="lidar_marker_localizer" name="lidar_marker_localizer" output="log">
+    <remap from="~/input/pointcloud_ex" 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/landmark_based_localizer/lidar_marker_localizer/package.xml

@@ -0,0 +1,41 @@
+<?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>lidar_marker_localizer</name>
+  <version>0.0.0</version>
+  <description>The 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_point_types</depend>
+  <depend>landmark_manager</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>localization_util</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_msgs</depend>
+  <depend>pcl_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_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>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>