From 291e2702823896ddfb63e2b651554b4a364d602b Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 3 Oct 2024 22:27:25 +0900 Subject: [PATCH 1/5] fix(compare_map_segmentation): throw runtime error when using non-split map pointcloud for DynamicMapLoader Signed-off-by: badai-nguyen --- .../config/distance_based_compare_map_filter.param.yaml | 1 + .../voxel_based_approximate_compare_map_filter.param.yaml | 1 + .../config/voxel_based_compare_map_filter.param.yaml | 1 + .../voxel_distance_based_compare_map_filter.param.yaml | 1 + .../schema/distance_based_compare_map_filter.schema.json | 8 +++++++- ...voxel_based_approximate_compare_map_filter.schema.json | 8 +++++++- .../schema/voxel_based_compare_map_filter.schema.json | 8 +++++++- .../voxel_distance_based_compare_map_filter.schema.json | 8 +++++++- .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 1 + .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 6 ++++++ 10 files changed, 39 insertions(+), 4 deletions(-) diff --git a/perception/autoware_compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml index f95c0b0c94b92..12fd2e1f27f0a 100644 --- a/perception/autoware_compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml +++ b/perception/autoware_compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml @@ -6,3 +6,4 @@ map_update_distance_threshold: 10.0 map_loader_radius: 150.0 publish_debug_pcd: False + max_map_grid_size: 100.0 diff --git a/perception/autoware_compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml index d54f7a96ecda0..56ab652265de5 100644 --- a/perception/autoware_compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml +++ b/perception/autoware_compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml @@ -7,3 +7,4 @@ map_update_distance_threshold: 10.0 map_loader_radius: 150.0 publish_debug_pcd: False + max_map_grid_size: 100.0 diff --git a/perception/autoware_compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml index d54f7a96ecda0..56ab652265de5 100644 --- a/perception/autoware_compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml +++ b/perception/autoware_compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml @@ -7,3 +7,4 @@ map_update_distance_threshold: 10.0 map_loader_radius: 150.0 publish_debug_pcd: False + max_map_grid_size: 100.0 diff --git a/perception/autoware_compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml index d54f7a96ecda0..56ab652265de5 100644 --- a/perception/autoware_compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml +++ b/perception/autoware_compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml @@ -7,3 +7,4 @@ map_update_distance_threshold: 10.0 map_loader_radius: 150.0 publish_debug_pcd: False + max_map_grid_size: 100.0 diff --git a/perception/autoware_compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json index b389e496fc477..75be17a3ec6b3 100644 --- a/perception/autoware_compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json +++ b/perception/autoware_compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json @@ -35,6 +35,11 @@ "type": "boolean", "default": "false", "description": "Publish a downsampled map pointcloud for debugging" + }, + "max_map_grid_size": { + "type": "number", + "default": "100.0", + "description": "Threshold of grid size to split map pointcloud" } }, "required": [ @@ -43,7 +48,8 @@ "timer_interval_ms", "map_update_distance_threshold", "map_loader_radius", - "publish_debug_pcd" + "publish_debug_pcd", + "max_map_grid_size" ], "additionalProperties": false } diff --git a/perception/autoware_compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json index 1b2e8ba5d51f8..f9a9d3a877adf 100644 --- a/perception/autoware_compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json +++ b/perception/autoware_compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json @@ -40,6 +40,11 @@ "type": "boolean", "default": "false", "description": "Publish a downsampled map pointcloud for debugging" + }, + "max_map_grid_size": { + "type": "number", + "default": "100.0", + "description": "Threshold of grid size to split map pointcloud" } }, "required": [ @@ -49,7 +54,8 @@ "timer_interval_ms", "map_update_distance_threshold", "map_loader_radius", - "publish_debug_pcd" + "publish_debug_pcd", + "max_map_grid_size" ], "additionalProperties": false } diff --git a/perception/autoware_compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json index 8cf95b8a3098e..326cc867bfd30 100644 --- a/perception/autoware_compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json +++ b/perception/autoware_compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json @@ -40,6 +40,11 @@ "type": "boolean", "default": "false", "description": "Publish a downsampled map pointcloud for debugging" + }, + "max_map_grid_size": { + "type": "number", + "default": "100.0", + "description": "Threshold of grid size to split map pointcloud" } }, "required": [ @@ -49,7 +54,8 @@ "timer_interval_ms", "map_update_distance_threshold", "map_loader_radius", - "publish_debug_pcd" + "publish_debug_pcd", + "max_map_grid_size" ], "additionalProperties": false } diff --git a/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json index d743fb45cfd60..3380fed1db309 100644 --- a/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json +++ b/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json @@ -40,6 +40,11 @@ "type": "boolean", "default": "false", "description": "Publish a downsampled map pointcloud for debugging" + }, + "max_map_grid_size": { + "type": "number", + "default": "100.0", + "description": "Threshold of grid size to split map pointcloud" } }, "required": [ @@ -49,7 +54,8 @@ "timer_interval_ms", "map_update_distance_threshold", "map_loader_radius", - "publish_debug_pcd" + "publish_debug_pcd", + "max_map_grid_size" ], "additionalProperties": false } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index 195e09e4b406e..a36372efe4428 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -293,6 +293,7 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( auto timer_interval_ms = node->declare_parameter("timer_interval_ms"); map_update_distance_threshold_ = node->declare_parameter("map_update_distance_threshold"); map_loader_radius_ = node->declare_parameter("map_loader_radius"); + max_map_grid_size_ = node->declare_parameter("max_map_grid_size"); auto main_sub_opt = rclcpp::SubscriptionOptions(); main_sub_opt.callback_group = main_callback_group; sub_kinematic_state_ = node->create_subscription( diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 4a044596b3dee..08aeae630bd97 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -152,6 +152,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader rclcpp::TimerBase::SharedPtr map_update_timer_; double map_update_distance_threshold_; double map_loader_radius_; + double max_map_grid_size_; rclcpp::Client::SharedPtr map_update_client_; rclcpp::CallbackGroup::SharedPtr client_callback_group_; @@ -270,6 +271,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader { map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) { + throw std::runtime_error( + "Map was not split or split map grid size is too large. Split map with smaller grid."); + return; + } origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_); origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_); From 50da316296e12aa3f76cfa4cc8b24e73f226846b Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 3 Oct 2024 22:40:31 +0900 Subject: [PATCH 2/5] chore: typo Signed-off-by: badai-nguyen --- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 08aeae630bd97..3a01142cca767 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -274,7 +274,6 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) { throw std::runtime_error( "Map was not split or split map grid size is too large. Split map with smaller grid."); - return; } origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_); From bcd2015925c4228133a4c6e73895cce7f4029f34 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 3 Oct 2024 22:43:47 +0900 Subject: [PATCH 3/5] fix: launch Signed-off-by: badai-nguyen --- .../detection/filter/pointcloud_map_filter.launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py index 4fda3521c07e9..e9c0dbec1c3a3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py @@ -39,6 +39,7 @@ def __init__(self, context): self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"] self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"] + self.max_map_grid_size = self.pointcloud_map_filter_param["max_map_grid_size"] self.map_update_distance_threshold = self.pointcloud_map_filter_param[ "map_update_distance_threshold" ] @@ -124,6 +125,7 @@ def create_compare_map_pipeline(self): "map_update_distance_threshold": self.map_update_distance_threshold, "map_loader_radius": self.map_loader_radius, "publish_debug_pcd": self.publish_debug_pcd, + "max_map_grid_size": self.max_map_grid_size, "input_frame": "map", } ], From b5dbabf6d178caf5f6860c5fbfd508e7a1967b0f Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 4 Oct 2024 08:15:16 +0900 Subject: [PATCH 4/5] Update perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json Co-authored-by: Yoshi Ri --- .../schema/voxel_distance_based_compare_map_filter.schema.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json index 3380fed1db309..27eb38d7449d7 100644 --- a/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json +++ b/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json @@ -44,7 +44,7 @@ "max_map_grid_size": { "type": "number", "default": "100.0", - "description": "Threshold of grid size to split map pointcloud" + "description": "Maximum size of the pcd map with dynamic map loading." } }, "required": [ From 1416bfbe79b8275c573459b38c42e09b72f42b62 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 4 Oct 2024 08:20:37 +0900 Subject: [PATCH 5/5] fix: change to RCLCPP_ERROR Signed-off-by: badai-nguyen --- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 3a01142cca767..f860e7e6c87fd 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -272,8 +272,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) { - throw std::runtime_error( - "Map was not split or split map grid size is too large. Split map with smaller grid."); + RCLCPP_ERROR( + logger_, + "Map was not split or split map grid size is too large. Split map with grid size smaller " + "than %f", + max_map_grid_size_); } origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_);