feat(elevation_map_loader): reduce memory usage of elevation_map_loader

perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp

@@ -73,7 +73,8 @@ class ElevationMapLoaderNode : public rclcpp::Node
   void publish();
   void createElevationMap();
   void setVerbosityLevelToDebugIfFlagSet();
-  void createElevationMapFromPointcloud();
+  void createElevationMapFromPointcloud(
+    const pcl::shared_ptr<grid_map::GridMapPclLoader> & grid_map_pcl_loader_);
   tier4_autoware_utils::LinearRing2d getConvexHull(
     const pcl::PointCloud<pcl::PointXYZ>::Ptr & input_cloud);
   lanelet::ConstLanelets getIntersectedLanelets(
@@ -97,7 +98,7 @@ class ElevationMapLoaderNode : public rclcpp::Node
   bool use_inpaint_;
   float inpaint_radius_;
   bool use_elevation_map_cloud_publisher_;
-  pcl::shared_ptr<grid_map::GridMapPclLoader> grid_map_pcl_loader_;
+  std::string param_file_path;
 
   DataManager data_manager_;
   struct LaneFilter

perception/elevation_map_loader/src/elevation_map_loader_node.cpp

@@ -50,7 +50,7 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio
 : Node("elevation_map_loader", options)
 {
   layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation"));
-  std::string param_file_path = this->declare_parameter("param_file_path", "path_default");
+  this->param_file_path = this->declare_parameter("param_file_path", "path_default");
   map_frame_ = this->declare_parameter("map_frame", "map");
   use_inpaint_ = this->declare_parameter("use_inpaint", true);
   inpaint_radius_ = this->declare_parameter("inpaint_radius", 0.3);
@@ -67,11 +67,6 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio
   lane_filter_.voxel_size_y_ = declare_parameter("lane_filter_voxel_size_y", 0.04);
   lane_filter_.voxel_size_z_ = declare_parameter("lane_filter_voxel_size_z", 0.04);
 
-  auto grid_map_logger = rclcpp::get_logger("grid_map_logger");
-  grid_map_logger.set_level(rclcpp::Logger::Level::Error);
-  grid_map_pcl_loader_ = pcl::make_shared<grid_map::GridMapPclLoader>(grid_map_logger);
-  grid_map_pcl_loader_->loadParameters(param_file_path);
-
   rclcpp::QoS durable_qos{1};
   durable_qos.transient_local();
   pub_elevation_map_ =
@@ -153,9 +148,11 @@ void ElevationMapLoaderNode::onPointcloudMap(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map)
 {
   RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map");
-  pcl::PointCloud<pcl::PointXYZ> map_pcl;
-  pcl::fromROSMsg<pcl::PointXYZ>(*pointcloud_map, map_pcl);
-  data_manager_.map_pcl_ptr_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
+  {
+    pcl::PointCloud<pcl::PointXYZ> map_pcl;
+    pcl::fromROSMsg<pcl::PointXYZ>(*pointcloud_map, map_pcl);
+    data_manager_.map_pcl_ptr_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
+  }
   if (data_manager_.isInitialized()) {
     publish();
   }
@@ -177,6 +174,11 @@ void ElevationMapLoaderNode::onVectorMap(
 
 void ElevationMapLoaderNode::createElevationMap()
 {
+  auto grid_map_logger = rclcpp::get_logger("grid_map_logger");
+  grid_map_logger.set_level(rclcpp::Logger::Level::Error);
+  pcl::shared_ptr<grid_map::GridMapPclLoader> grid_map_pcl_loader_ =
+    pcl::make_shared<grid_map::GridMapPclLoader>(grid_map_logger);
+  grid_map_pcl_loader_->loadParameters(this->param_file_path);
   if (lane_filter_.use_lane_filter_) {
     const auto convex_hull = getConvexHull(data_manager_.map_pcl_ptr_);
     lanelet::ConstLanelets intersected_lanelets =
@@ -187,15 +189,16 @@ void ElevationMapLoaderNode::createElevationMap()
   } else {
     grid_map_pcl_loader_->setInputCloud(data_manager_.map_pcl_ptr_);
   }
-  createElevationMapFromPointcloud();
+  createElevationMapFromPointcloud(grid_map_pcl_loader_);
   elevation_map_ = grid_map_pcl_loader_->getGridMap();
   if (use_inpaint_) {
     inpaintElevationMap(inpaint_radius_);
   }
   saveElevationMap();
 }
 
-void ElevationMapLoaderNode::createElevationMapFromPointcloud()
+void ElevationMapLoaderNode::createElevationMapFromPointcloud(
+  const pcl::shared_ptr<grid_map::GridMapPclLoader> & grid_map_pcl_loader_)
 {
   const auto start = std::chrono::high_resolution_clock::now();
   grid_map_pcl_loader_->preProcessInputCloud();