feat(elevation_map_loader): reduce memory usage of elevation_map_load…

…er (autowarefoundation#2571) * feat: reduce memory usage of elevation_map_loader Signed-off-by: Shin-kyoto <[email protected]> * chore: remove unnecessary comment Signed-off-by: Shin-kyoto <[email protected]> * fix: modify variables' name Signed-off-by: Shin-kyoto <[email protected]> Signed-off-by: Shin-kyoto <[email protected]>
Shin-kyoto · Feb 15, 2023 · e7622ca · e7622ca
1 parent cba06ae
commit e7622ca
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 19 deletions.
diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/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

diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/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");
+  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,30 +174,36 @@ 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(param_file_path_);
   if (lane_filter_.use_lane_filter_) {
     const auto convex_hull = getConvexHull(data_manager_.map_pcl_ptr_);
     lanelet::ConstLanelets intersected_lanelets =
       getIntersectedLanelets(convex_hull, lane_filter_.road_lanelets_);
     pcl::PointCloud<pcl::PointXYZ>::Ptr lane_filtered_map_pcl_ptr =
       getLaneFilteredPointCloud(intersected_lanelets, data_manager_.map_pcl_ptr_);
-    grid_map_pcl_loader_->setInputCloud(lane_filtered_map_pcl_ptr);
+    grid_map_pcl_loader->setInputCloud(lane_filtered_map_pcl_ptr);
   } else {
-    grid_map_pcl_loader_->setInputCloud(data_manager_.map_pcl_ptr_);
+    grid_map_pcl_loader->setInputCloud(data_manager_.map_pcl_ptr_);
   }
-  createElevationMapFromPointcloud();
-  elevation_map_ = grid_map_pcl_loader_->getGridMap();
+  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();
-  grid_map_pcl_loader_->initializeGridMapGeometryFromInputCloud();
-  grid_map_pcl_loader_->addLayerFromInputCloud(layer_name_);
+  grid_map_pcl_loader->preProcessInputCloud();
+  grid_map_pcl_loader->initializeGridMapGeometryFromInputCloud();
+  grid_map_pcl_loader->addLayerFromInputCloud(layer_name_);
   grid_map::grid_map_pcl::printTimeElapsedToRosInfoStream(
     start, "Finish creating elevation map. Total time: ", this->get_logger());
 }