-
Notifications
You must be signed in to change notification settings - Fork 639
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.
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
feat(elevation_map_loader): reduce memory usage of elevation_map_loader #2571
Changes from 3 commits
94fdb62
927b6f4
263086e
4336653
38d78b0
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change | ||||
---|---|---|---|---|---|---|
|
@@ -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; | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. or not making a member, using |
||||||
|
||||||
DataManager data_manager_; | ||||||
struct LaneFilter | ||||||
|
Original file line number | Diff line number | Diff line change | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
|
@@ -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"); | ||||||||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. it is better to align with other variables. |
||||||||||||||
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); | ||||||||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
|
||||||||||||||
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_); | ||||||||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
|
||||||||||||||
elevation_map_ = grid_map_pcl_loader_->getGridMap(); | ||||||||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
|
||||||||||||||
if (use_inpaint_) { | ||||||||||||||
inpaintElevationMap(inpaint_radius_); | ||||||||||||||
} | ||||||||||||||
saveElevationMap(); | ||||||||||||||
} | ||||||||||||||
|
||||||||||||||
void ElevationMapLoaderNode::createElevationMapFromPointcloud() | ||||||||||||||
void ElevationMapLoaderNode::createElevationMapFromPointcloud( | ||||||||||||||
const pcl::shared_ptr<grid_map::GridMapPclLoader> & grid_map_pcl_loader_) | ||||||||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
|
||||||||||||||
{ | ||||||||||||||
const auto start = std::chrono::high_resolution_clock::now(); | ||||||||||||||
grid_map_pcl_loader_->preProcessInputCloud(); | ||||||||||||||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.