Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(elevation_map_loader): reduce memory usage of elevation_map_loader #2571

Merged
merged 5 commits into from
Jan 12, 2023
Merged

feat(elevation_map_loader): reduce memory usage of elevation_map_loader #2571

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
94fdb62
feat: reduce memory usage of elevation_map_loader
Shin-kyoto Dec 22, 2022
927b6f4
chore: remove unnecessary comment
Shin-kyoto Dec 22, 2022
263086e
Merge branch 'main' into feature/reduce_memory_usage
Shin-kyoto Dec 23, 2022
4336653
fix: modify variables' name
Shin-kyoto Dec 26, 2022
38d78b0
Merge branch 'main' into feature/reduce_memory_usage
Shin-kyoto Jan 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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
Expand Down
37 changes: 20 additions & 17 deletions perception/elevation_map_loader/src/elevation_map_loader_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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_ =
Expand Down Expand Up @@ -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();
}
Expand All @@ -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());
}
Expand Down