From 5aced52b8d7e491fe446e4fa583762e24c43aad3 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 13 Mar 2024 14:02:53 +0900 Subject: [PATCH 1/5] chore: change default of low_height_crop filter use (#918) Signed-off-by: badai-nguyen --- .../launch/components/tier4_perception_component.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 3b0f8eca4..d4d941da4 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -19,6 +19,7 @@ + Date: Wed, 13 Mar 2024 17:47:21 +0900 Subject: [PATCH 2/5] feat(pointcloud_preprocessor, probabilistic_occupancy_grid_map): enable multi lidar occupancy grid map creation pipeline (#740) * add multi lidar pointcloud based ogm creation Signed-off-by: yoshiri * enable sensing launch to control concatenate node Signed-off-by: yoshiri * style(pre-commit): autofix * refactor : change concatenate node parameter name Signed-off-by: yoshiri * chore: set single lidar ogm to be default Signed-off-by: yoshiri * feat: update multi_lidar_ogm param file Signed-off-by: yoshiri * chore: remove sensing launch changes because it does not needed Signed-off-by: yoshiri * chore: fix multi lidar settings for sample sensor kit Signed-off-by: yoshiri --------- Signed-off-by: yoshiri Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- ...tcloud_based_occupancy_grid_map.param.yaml | 53 +++++++++++++++++++ ...nchronized_grid_map_fusion_node.param.yaml | 20 +++++++ .../tier4_perception_component.launch.xml | 6 ++- 3 files changed, 78 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 000000000..f9a1c4f93 --- /dev/null +++ b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml @@ -0,0 +1,53 @@ +# 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 + raw_pointcloud_topics: # put each sensor's pointcloud topic + - "/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" + # 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 000000000..f8a2dc2fb --- /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 d4d941da4..8418afabd 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, 13 Mar 2024 18:50:04 +0900 Subject: [PATCH 3/5] chore(duplicated_node_checker): print duplication name (#888) Signed-off-by: Mamoru Sobue --- .../duplicated_node_checker/duplicated_node_checker.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/duplicated_node_checker/duplicated_node_checker.param.yaml b/autoware_launch/config/system/duplicated_node_checker/duplicated_node_checker.param.yaml index 54b4f691b..96dce7eb3 100644 --- a/autoware_launch/config/system/duplicated_node_checker/duplicated_node_checker.param.yaml +++ b/autoware_launch/config/system/duplicated_node_checker/duplicated_node_checker.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: update_rate: 10.0 - add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg + add_duplicated_node_names_to_msg: true # if true, duplicated node names are added to msg From b37411114f38cf54b8800de297cc39c7c1e1034d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 14 Mar 2024 00:18:02 +0900 Subject: [PATCH 4/5] feat: add marker for control's stop reason, false by default (#912) Signed-off-by: Takayuki Murooka --- autoware_launch/rviz/autoware.rviz | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 9b998dc29..8b305e3e4 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2374,6 +2374,19 @@ Visualization Manager: Constant Color: false Scale: 0.30000001192092896 Value: false + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Stop Reason + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/longitudinal/stop_reason + Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Debug/MPC From f4778f96f820d0fd2ee7b90f107dfb9aff048a09 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 14 Mar 2024 13:04:23 +0900 Subject: [PATCH 5/5] feat(crosswalk): increase minimum occlusion size that causes slowdown to 1m (#909) Signed-off-by: Maxime CLEMENT --- .../behavior_velocity_planner/crosswalk.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 87140169b..ac18c935d 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -75,7 +75,7 @@ occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space slow_down_velocity: 1.0 # [m/s] time_buffer: 0.5 # [s] consecutive time with/without an occlusion to add/remove the slowdown - min_size: 0.5 # [m] minimum size of an occlusion (square side size) + min_size: 1.0 # [m] minimum size of an occlusion (square side size) free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid ignore_with_red_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored