diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index 76026167f81a7..fa94afbf12a0b 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -1,4 +1,8 @@ /**: ros__parameters: enable_whole_load: true - enable_partial_load: true + enable_downsampled_whole_load: false + enable_partial_load: false + + # only used when downsample_whole_load enabled + leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/README.md b/map/map_loader/README.md index baafe9729387e..1683fd472e74a 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -10,12 +10,17 @@ This package provides the features of loading various maps. Currently, it supports the following two types: - Publish raw pointcloud map +- Publish downsampled pointcloud map - Send partial pointcloud map loading via ROS 2 service #### Publish raw pointcloud map (ROS 2 topic) The node publishes the raw pointcloud map loaded from the `.pcd` file(s). +#### Publish downsampled pointcloud map (ROS 2 topic) + +The node publishes the downsampled pointcloud map loaded from the `.pcd` file(s). You can specify the downsample resolution by changing the `leaf_size` parameter. + #### Send partial pointcloud map (ROS 2 service) Here, we assume that the pointcloud maps are divided into grids. @@ -25,14 +30,17 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com ### Parameters -| Name | Type | Description | Default value | -| :------------------ | :--- | :--------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | +| Name | Type | Description | Default value | +| :---------------------------- | :---- | :-------------------------------------------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | false | +| leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map +- `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map --- diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index 76026167f81a7..fa94afbf12a0b 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -1,4 +1,8 @@ /**: ros__parameters: enable_whole_load: true - enable_partial_load: true + enable_downsampled_whole_load: false + enable_partial_load: false + + # only used when downsample_whole_load enabled + leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index e83b0df4d21c6..cd1a75fad5a03 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -15,14 +15,33 @@ #include "pointcloud_map_loader_module.hpp" #include +#include #include #include #include #include +sensor_msgs::msg::PointCloud2 downsample( + const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) +{ + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(msg_input, *pcl_input); + pcl::VoxelGrid filter; + filter.setInputCloud(pcl_input); + filter.setLeafSize(leaf_size, leaf_size, leaf_size); + filter.filter(*pcl_output); + + sensor_msgs::msg::PointCloud2 msg_output; + pcl::toROSMsg(*pcl_output, msg_output); + msg_output.header = msg_input.header; + return msg_output; +} + PointcloudMapLoaderModule::PointcloudMapLoaderModule( - rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name) + rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name, + const bool use_downsample) : logger_(node->get_logger()) { rclcpp::QoS durable_qos{1}; @@ -30,7 +49,13 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( pub_pointcloud_map_ = node->create_publisher(publisher_name, durable_qos); - sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths); + sensor_msgs::msg::PointCloud2 pcd; + if (use_downsample) { + const float leaf_size = node->declare_parameter("leaf_size"); + pcd = loadPCDFiles(pcd_paths, leaf_size); + } else { + pcd = loadPCDFiles(pcd_paths, boost::none); + } if (pcd.width == 0) { RCLCPP_ERROR(logger_, "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size()); @@ -42,7 +67,7 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( } sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( - const std::vector & pcd_paths) const + const std::vector & pcd_paths, const boost::optional leaf_size) const { sensor_msgs::msg::PointCloud2 whole_pcd; sensor_msgs::msg::PointCloud2 partial_pcd; @@ -59,6 +84,10 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); } + if (leaf_size) { + partial_pcd = downsample(partial_pcd, leaf_size.get()); + } + if (whole_pcd.width == 0) { whole_pcd = partial_pcd; } else { diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 35e644efb8fe3..4cd0e981597c6 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -29,13 +29,14 @@ class PointcloudMapLoaderModule public: explicit PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, - const std::string publisher_name); + const std::string publisher_name, const bool use_downsample); private: rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths) const; + sensor_msgs::msg::PointCloud2 loadPCDFiles( + const std::vector & pcd_paths, const boost::optional leaf_size) const; }; #endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index e6a93694737cf..b42be7308b67b 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -50,11 +50,19 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt const auto pcd_paths = getPcdPaths(declare_parameter>("pcd_paths_or_directory")); bool enable_whole_load = declare_parameter("enable_whole_load"); + bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); if (enable_whole_load) { std::string publisher_name = "output/pointcloud_map"; - pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name); + pcd_map_loader_ = + std::make_unique(this, pcd_paths, publisher_name, false); + } + + if (enable_downsample_whole_load) { + std::string publisher_name = "output/debug/downsampled_pointcloud_map"; + downsampled_pcd_map_loader_ = + std::make_unique(this, pcd_paths, publisher_name, true); } if (enable_partial_load) { diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index 7a297c900be12..a7c289dd0f9a8 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -41,6 +41,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::map pcd_metadata_dict_; std::unique_ptr pcd_map_loader_; + std::unique_ptr downsampled_pcd_map_loader_; std::unique_ptr partial_map_loader_; std::vector getPcdPaths(