From 81800f55296e3801cbd7afa623944782e37ff1de Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 23 Aug 2024 16:15:19 +0900 Subject: [PATCH] feat(localization): add lidar_marker_localizer (#861) * add config files Signed-off-by: Yamato Ando * style(pre-commit): autofix * add param marker_height_from_ground Signed-off-by: Yamato Ando * save log param Signed-off-by: Yamato Ando * apply PointXYZIRC Signed-off-by: Yamato Ando * to pass spell-check Signed-off-by: Yamato Ando * refactor Signed-off-by: Yamato Ando * change flag Signed-off-by: Yamato Ando * fix typo Signed-off-by: Yamato Ando --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro --- .../lidar_marker_localizer.param.yaml | 42 +++++++++++++++++++ ...op_box_filter_measurement_range.param.yaml | 11 +++++ .../ring_filter.param.yaml | 9 ++++ .../tier4_localization_component.launch.xml | 8 ++++ 4 files changed, 70 insertions(+) create mode 100644 autoware_launch/config/localization/lidar_marker_localizer/lidar_marker_localizer.param.yaml create mode 100644 autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml create mode 100644 autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/ring_filter.param.yaml diff --git a/autoware_launch/config/localization/lidar_marker_localizer/lidar_marker_localizer.param.yaml b/autoware_launch/config/localization/lidar_marker_localizer/lidar_marker_localizer.param.yaml new file mode 100644 index 0000000000..1c04d51e5d --- /dev/null +++ b/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 diff --git a/autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml b/autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml new file mode 100644 index 0000000000..6b3986540d --- /dev/null +++ b/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 diff --git a/autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/ring_filter.param.yaml b/autoware_launch/config/localization/lidar_marker_localizer/pointcloud_preprocessor/ring_filter.param.yaml new file mode 100644 index 0000000000..7d87f797c1 --- /dev/null +++ b/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 diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index 7dfe0a3dea..1d5f406518 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -51,6 +51,14 @@ + + + + +