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 index aa41c6d8c0b03..f5c294dd27207 100644 --- 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_pcl_loader); tier4_autoware_utils::LinearRing2d getConvexHull( const pcl::PointCloud::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_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 index 4474fe2740467..5434447a78c1f 100644 --- 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_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 map_pcl; - pcl::fromROSMsg(*pointcloud_map, map_pcl); - data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + { + pcl::PointCloud map_pcl; + pcl::fromROSMsg(*pointcloud_map, map_pcl); + data_manager_.map_pcl_ptr_ = pcl::make_shared>(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_pcl_loader = + pcl::make_shared(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::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_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()); }