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..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 @@ -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": "Maximum size of the pcd map with dynamic map loading." } }, "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..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 @@ -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,13 @@ 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_) { + 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_); origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_);