From 7b793e848d788924789dbaa7bc91f7477f3678e7 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 15 Dec 2023 18:45:50 +0900 Subject: [PATCH 1/8] add multi lidar pointcloud based ogm creation Signed-off-by: yoshiri --- ...tcloud_based_occupancy_grid_map.param.yaml | 58 +++++++++++++++++++ ...nchronized_grid_map_fusion_node.param.yaml | 20 +++++++ .../tier4_perception_component.launch.xml | 2 +- 3 files changed, 79 insertions(+), 1 deletion(-) create mode 100644 autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml create mode 100644 autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml diff --git a/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml new file mode 100644 index 0000000000..f7be86356a --- /dev/null +++ b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml @@ -0,0 +1,58 @@ +# sample grid map fusion parameters for sample sensor kit +/**: + ros__parameters: + # shared parameters + shared_config: + map_frame: "map" + base_link_frame: "base_link" + # center of the grid map + gridmap_origin_frame: "base_link" + + map_resolution: 0.5 # [m] + map_length_x: 150.0 # [m] + map_length_y: 150.0 # [m] + + # each grid map parameters + ogm_creation_config: + height_filter: + use_height_filter: true + min_height: -1.0 + max_height: 2.0 + enable_single_frame_mode: true + # use sensor pointcloud to filter obstacle pointcloud + filter_obstacle_pointcloud_by_raw_pointcloud: true + + grid_map_type: "OccupancyGridMapFixedBlindSpot" + OccupancyGridMapFixedBlindSpot: + distance_margin: 1.0 + OccupancyGridMapProjectiveBlindSpot: + projection_dz_threshold: 0.01 # [m] for avoiding null division + obstacle_separation_threshold: 1.0 # [m] fill the interval between obstacles with unknown for this length + pub_debug_grid: false + + # parameter settings for ogm fusion + fusion_config: + # following parameters are shared: map_frame, base_link_frame, gridmap_origin_frame, map_resolution, map_length + # Setting1: tune ogm creation parameters + obstacle_pointcloud_topic: "/perception/obstacle_segmentation/single_frame/pointcloud_raw" + raw_pointcloud_topics: # put each sensor's pointcloud topic + - "/sensing/lidar/top/pointcloud_synchronized" + - "/sensing/lidar/left/pointcloud_synchronized" + - "/sensing/lidar/right/pointcloud_synchronized" + fusion_input_ogm_topics: + - "/perception/occupancy_grid_map/top_lidar/map" + - "/perception/occupancy_grid_map/left_lidar/map" + - "/perception/occupancy_grid_map/right_lidar/map" + each_ogm_sensor_frames: + - "velodyne_top" + - "velodyne_left" + - "velodyne_right" + # reliability of each sensor (0.0 ~ 1.0) only work with "log-odds" and "dempster-shafer" + input_ogm_reliabilities: + - 1.0 + - 0.6 + - 0.6 + + # Setting2: tune ogm fusion parameters + ## choose fusion method from ["overwrite", "log-odds", "dempster-shafer"] + fusion_method: "overwrite" diff --git a/autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml new file mode 100644 index 0000000000..f8a2dc2fbc --- /dev/null +++ b/autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # 1. fusion parameters + fusion_input_ogm_topics: ["topic1", "topic2"] + input_ogm_reliabilities: [0.8, 0.2] + fusion_method: "overwrite" # choose from ["overwrite", "log-odds", "dempster-shafer"] + + # 2. synchronization settings + match_threshold_sec: 0.01 # 10ms + timeout_sec: 0.1 # 100ms + input_offset_sec: [0.0, 0.0] # no offset + + # 3. settings for fused fusion map + # remember resolution and map size should be same with input maps + map_frame_: "map" + base_link_frame_: "base_link" + grid_map_origin_frame_: "base_link" + fusion_map_length_x: 100.0 + fusion_map_length_y: 100.0 + fusion_map_resolution: 0.5 diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 49156fba51..d89b09d042 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -1,6 +1,6 @@ - + Date: Fri, 15 Dec 2023 18:46:09 +0900 Subject: [PATCH 2/8] enable sensing launch to control concatenate node Signed-off-by: yoshiri --- .../launch/components/tier4_sensing_component.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/launch/components/tier4_sensing_component.launch.xml b/autoware_launch/launch/components/tier4_sensing_component.launch.xml index fe520e1fdc..1c0f4c0a95 100644 --- a/autoware_launch/launch/components/tier4_sensing_component.launch.xml +++ b/autoware_launch/launch/components/tier4_sensing_component.launch.xml @@ -1,5 +1,7 @@ + + From a34e9e1d63811727bf3182a187e0ab4677fc15a2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 15 Dec 2023 09:52:27 +0000 Subject: [PATCH 3/8] style(pre-commit): autofix --- .../launch/components/tier4_perception_component.launch.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index d89b09d042..4fc4df91fe 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -1,6 +1,10 @@ - + Date: Wed, 20 Dec 2023 14:34:38 +0900 Subject: [PATCH 4/8] refactor : change concatenate node parameter name Signed-off-by: yoshiri --- .../launch/components/tier4_sensing_component.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/components/tier4_sensing_component.launch.xml b/autoware_launch/launch/components/tier4_sensing_component.launch.xml index 1c0f4c0a95..37636385d2 100644 --- a/autoware_launch/launch/components/tier4_sensing_component.launch.xml +++ b/autoware_launch/launch/components/tier4_sensing_component.launch.xml @@ -1,6 +1,6 @@ - + From 03e18146065872ba58b28c475500ef0f00c0e7c1 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Wed, 20 Dec 2023 14:38:19 +0900 Subject: [PATCH 5/8] chore: set single lidar ogm to be default Signed-off-by: yoshiri --- .../launch/components/tier4_perception_component.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 4fc4df91fe..d9769ea2ca 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -2,7 +2,7 @@ From 05da320ee2f56d4a4dee70ad8158aaad9d24bfde Mon Sep 17 00:00:00 2001 From: yoshiri Date: Mon, 4 Mar 2024 16:03:14 +0900 Subject: [PATCH 6/8] feat: update multi_lidar_ogm param file Signed-off-by: yoshiri --- ...idar_pointcloud_based_occupancy_grid_map.param.yaml | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml index f7be86356a..0badf26c75 100644 --- a/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml +++ b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml @@ -36,17 +36,13 @@ # Setting1: tune ogm creation parameters obstacle_pointcloud_topic: "/perception/obstacle_segmentation/single_frame/pointcloud_raw" raw_pointcloud_topics: # put each sensor's pointcloud topic - - "/sensing/lidar/top/pointcloud_synchronized" - - "/sensing/lidar/left/pointcloud_synchronized" - - "/sensing/lidar/right/pointcloud_synchronized" + - "/sensing/lidar/top/pointcloud" + - "/sensing/lidar/left/pointcloud" + - "/sensing/lidar/right/pointcloud" fusion_input_ogm_topics: - "/perception/occupancy_grid_map/top_lidar/map" - "/perception/occupancy_grid_map/left_lidar/map" - "/perception/occupancy_grid_map/right_lidar/map" - each_ogm_sensor_frames: - - "velodyne_top" - - "velodyne_left" - - "velodyne_right" # reliability of each sensor (0.0 ~ 1.0) only work with "log-odds" and "dempster-shafer" input_ogm_reliabilities: - 1.0 From ab4832f528bb23de326f8d0bd6c0713f3a5747c1 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Mon, 4 Mar 2024 16:28:53 +0900 Subject: [PATCH 7/8] chore: remove sensing launch changes because it does not needed Signed-off-by: yoshiri --- .../launch/components/tier4_sensing_component.launch.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/autoware_launch/launch/components/tier4_sensing_component.launch.xml b/autoware_launch/launch/components/tier4_sensing_component.launch.xml index 37636385d2..fe520e1fdc 100644 --- a/autoware_launch/launch/components/tier4_sensing_component.launch.xml +++ b/autoware_launch/launch/components/tier4_sensing_component.launch.xml @@ -1,7 +1,5 @@ - - From 14b832cc2179f1d66c0b4f10607d905880e85112 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Wed, 13 Mar 2024 15:29:12 +0900 Subject: [PATCH 8/8] chore: fix multi lidar settings for sample sensor kit Signed-off-by: yoshiri --- .../multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml index 0badf26c75..f9a1c4f930 100644 --- a/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml +++ b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml @@ -34,7 +34,6 @@ fusion_config: # following parameters are shared: map_frame, base_link_frame, gridmap_origin_frame, map_resolution, map_length # Setting1: tune ogm creation parameters - obstacle_pointcloud_topic: "/perception/obstacle_segmentation/single_frame/pointcloud_raw" raw_pointcloud_topics: # put each sensor's pointcloud topic - "/sensing/lidar/top/pointcloud" - "/sensing/lidar/left/pointcloud"