fix(compare_map_segmentation): throw runtime error when using non-split map pointcloud for DynamicMapLoader (#9024)

* fix(compare_map_segmentation): throw runtime error when using non-split map pointcloud for DynamicMapLoader

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: launch

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* Update perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json

Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com>

* fix: change to RCLCPP_ERROR

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com>

Author: badai-nguyen

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

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

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

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

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
     }

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
     }

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
     }

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
     }

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<int>("timer_interval_ms");
   map_update_distance_threshold_ = node->declare_parameter<double>("map_update_distance_threshold");
   map_loader_radius_ = node->declare_parameter<double>("map_loader_radius");
+  max_map_grid_size_ = node->declare_parameter<double>("max_map_grid_size");
   auto main_sub_opt = rclcpp::SubscriptionOptions();
   main_sub_opt.callback_group = main_callback_group;
   sub_kinematic_state_ = node->create_subscription<nav_msgs::msg::Odometry>(

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<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::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_);