fix(compare_map_segmentation): throw runtime error when using non-spl…

…it 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>
autowarefoundation · Oct 17, 2024 · 37808bf · 37808bf
1 parent 2be4572
commit 37808bf
Show file tree

Hide file tree

Showing 10 changed files with 41 additions and 4 deletions.
diff --git a/...ion/autoware_compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml b/...ion/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/...are_compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/...are_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/...eption/autoware_compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/...eption/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/...toware_compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/...toware_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/...on/autoware_compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/...on/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/...re_compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/...re_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/...ption/autoware_compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/...ption/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/...oware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/...oware_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/...ion/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/...ion/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>(

diff --git a/...ion/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/...ion/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_);