refactor: refactoring

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

launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py

@@ -38,6 +38,12 @@ def __init__(self, context):
         self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"]
         self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"]
         self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"]
+        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.map_update_distance_threshold = self.pointcloud_map_filter_param[
+            "map_update_distance_threshold"
+        ]
+        self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"]
 
     def create_pipeline(self):
         if self.use_down_sample_filter:
@@ -61,11 +67,11 @@ def create_normal_pipeline(self):
                 parameters=[
                     {
                         "distance_threshold": self.distance_threshold,
+                        "timer_interval_ms": self.timer_interval_ms,
+                        "use_dynamic_map_loading": self.use_dynamic_map_loading,
+                        "map_update_distance_threshold": self.map_update_distance_threshold,
+                        "map_loader_radius": self.map_loader_radius,
                         "input_frame": "map",
-                        "timer_interval_ms": 100,
-                        "is_static_map": False,
-                        "map_update_distance_threshold": 10.0,
-                        "map_loader_radius": 150,
                     }
                 ],
                 extra_arguments=[

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp

@@ -169,7 +169,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   }
 
   inline void addMapCellAndFilter(
-    const autoware_map_msgs::msg::PointCloudMapCellWithID map_cell_to_add)
+    const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add)
   {
     auto exe_start_time = std::chrono::system_clock::now();
 
@@ -188,7 +188,6 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
     map_cell_voxel_grid_tmp.filter(*map_cell_downsampled_pc_ptr_tmp);
 
     MapGridVoxelInfo current_voxel_grid_list_item;
-    // current_voxel_grid_list_item.voxel_grid_filtered_pc = *map_cell_downsampled_pc_ptr_tmp;
     current_voxel_grid_list_item.min_b_x = map_cell_voxel_grid_tmp.get_min_p()[0];
     current_voxel_grid_list_item.min_b_y = map_cell_voxel_grid_tmp.get_min_p()[1];
     current_voxel_grid_list_item.max_b_x = map_cell_voxel_grid_tmp.get_max_p()[0];
@@ -200,10 +199,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
       map_cell_voxel_grid_tmp.get_divb_mul(), map_cell_voxel_grid_tmp.get_inverse_leaf_size());
 
     current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>);
-    for (size_t i = 0; i < map_cell_downsampled_pc_ptr_tmp->points.size(); ++i) {
-      current_voxel_grid_list_item.map_cell_pc_ptr->points.push_back(
-        map_cell_downsampled_pc_ptr_tmp->points.at(i));
-    }
+    // for (size_t i = 0; i < map_cell_downsampled_pc_ptr_tmp->points.size(); ++i) {
+    //   current_voxel_grid_list_item.map_cell_pc_ptr->points.push_back(
+    //     map_cell_downsampled_pc_ptr_tmp->points.at(i));
+    // }
+    current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp);
     // add
     (*mutex_ptr_).lock();
     current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});

perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp

@@ -38,18 +38,18 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
     stop_watch_ptr_->tic("processing_time");
   }
 
-  distance_threshold_ = static_cast<double>(declare_parameter("distance_threshold", 0.3));
-  bool is_static_map = static_cast<bool>(declare_parameter("is_static_map", true));
+  distance_threshold_ = declare_parameter<double>("distance_threshold");
+  bool use_dynamic_map_loading = declare_parameter<bool>("use_dynamic_map_loading");
 
   set_map_in_voxel_grid_ = false;
-  if (is_static_map) {
-    voxel_grid_map_looader_ = std::make_unique<VoxelGridStaticMapLoader>(
-      this, distance_threshold_, &tf_input_frame_, &mutex_);
-  } else {
+  if (use_dynamic_map_loading) {
     rclcpp::CallbackGroup::SharedPtr main_callback_group;
     main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
     voxel_grid_map_looader_ = std::make_unique<VoxelGridDynamicMapLoader>(
       this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group);
+  } else {
+    voxel_grid_map_looader_ = std::make_unique<VoxelGridStaticMapLoader>(
+      this, distance_threshold_, &tf_input_frame_, &mutex_);
   }
   tf_input_frame_ = *(voxel_grid_map_looader_->tf_map_input_frame_);
 }

perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp

@@ -137,10 +137,9 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
   rclcpp::CallbackGroup::SharedPtr main_callback_group)
 : VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex)
 {
-  auto timer_interval_ms = static_cast<int>(node->declare_parameter("timer_interval_ms", 500));
-  map_update_distance_threshold_ =
-    static_cast<double>(node->declare_parameter("map_update_distance_threshold", 10.0));
-  map_loader_radius_ = static_cast<double>(node->declare_parameter("map_loader_radius", 100));
+  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");
   auto main_sub_opt = rclcpp::SubscriptionOptions();
   main_sub_opt.callback_group = main_callback_group;