diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index a64d6a9ee2606..b27d3933f0d55 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -26,16 +26,6 @@ rclcpp_components_register_node(pointcloud_map_loader_node EXECUTABLE pointcloud_map_loader ) -ament_auto_add_library(elevation_map_loader_node SHARED - src/elevation_map_loader/elevation_map_loader_node.cpp -) -target_link_libraries(elevation_map_loader_node ${PCL_LIBRARIES}) - -rclcpp_components_register_node(elevation_map_loader_node - PLUGIN "ElevationMapLoaderNode" - EXECUTABLE elevation_map_loader -) - ament_auto_add_library(lanelet2_map_loader_node SHARED src/lanelet2_map_loader/lanelet2_map_loader_node.cpp ) @@ -66,6 +56,4 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch - config - data ) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index a07b7d35cbf2e..311d3560acfc6 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -52,114 +52,3 @@ lanelet2_map_visualization visualizes autoware_lanelet2_msgs/MapBin messages int ### Published Topics - ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz - ---- - -## elevation_map_loader - -### Feature - -Generate elevation_map from subscribed pointcloud_map and vector_map and publish it. -Save the generated elevation_map locally and load it from next time. - -The elevation value of each cell is the average value of z of the points of the lowest cluster. -Cells with No elevation value can be inpainted using the values of neighboring cells. - -

- -

- -### How to run - -`ros2 run map_loader elevation_map_loader --ros-args -p param_file_path:=path/to/elevation_map_parameters.yaml -p elevation_map_directory:=path/to/elevation_map_directory -p pointcloud_map_path:=path/to/pointcloud.pcd` - -### Subscribed Topics - -- input/pointcloud_map (sensor_msgs:PointCloud2) : PointCloud Map -- input/vector_map (autoware_lanelet2_msgs/MapBin) : binary data of Lanelet2 Map - -### Published Topics - -- output/elevation_map (grid_map_msgs/GridMap) : Elevation Map -- output/elevation_map_cloud (sensor_msgs:PointCloud2) : Pointcloud generated from the value of Elevation Map - -### Parameter description - -#### ROS parameters - -| Name | Type | Description | Default value | -| :-------------------------------- | :---------- | :--------------------------------------------------------------------------------------------------------- | :------------ | -| map_layer_name | std::string | elevation_map layer name | elevation | -| param_file_path | std::string | GridMap parameters config | path_default | -| elevation_map_file_path | std::string | elevation_map file (bag2) | path_default | -| map_frame | std::string | map_frame when loading elevation_map file | map | -| use_inpaint | bool | Whether to inpaint empty cells | true | -| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | -| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | -| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | -| lane_margin | float | Value of how much to expand the range of vector_map [m] | 0.5 | -| lane_height_diff_thresh | float | Only point clouds in the height range of this value from vector_map are used to generate elevation_map [m] | 1.0 | -| lane_filter_voxel_size_x | float | Voxel size x for calculating point clouds in vector_map [m] | 0.04 | -| lane_filter_voxel_size_y | float | Voxel size y for calculating point clouds in vector_map [m] | 0.04 | -| lane_filter_voxel_size_z | float | Voxel size z for calculating point clouds in vector_map [m] | 0.04 | - -#### GridMap parameters - -The parameters are described on `config/elevation_map_parameters.yaml`. - -##### General parameters - -| Name | Type | Description | Default value | -| :--------------------------------------------- | :--- | :----------------------------------------------------------------------------------------------------------- | :------------ | -| pcl_grid_map_extraction/num_processing_threads | int | Number of threads for processing grid map cells. Filtering of the raw input point cloud is not parallelized. | 12 | - -##### Grid map parameters - -See: - -Resulting grid map parameters. - -| Name | Type | Description | Default value | -| :------------------------------------------------------- | :---- | :----------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| pcl_grid_map_extraction/grid_map/min_num_points_per_cell | int | Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN. | 3 | -| pcl_grid_map_extraction/grid_map/resolution | float | Resolution of the grid map. Width and length are computed automatically. | 0.3 | - -#### Point Cloud Pre-processing Parameters - -##### Rigid body transform parameters - -Rigid body transform that is applied to the point cloud before computing elevation. - -| Name | Type | Description | Default value | -| :-------------------------------------------------- | :---- | :---------------------------------------------------------------------------------------------------------------------- | :------------ | -| pcl_grid_map_extraction/cloud_transform/translation | float | Translation (xyz) that is applied to the input point cloud before computing elevation. | 0.0 | -| pcl_grid_map_extraction/cloud_transform/rotation | float | Rotation (intrinsic rotation, convention X-Y'-Z'') that is applied to the input point cloud before computing elevation. | 0.0 | - -##### Cluster extraction parameters - -Cluster extraction is based on pcl algorithms. See for more details. - -| Name | Type | Description | Default value | -| :----------------------------------------------------------- | :---- | :------------------------------------------------------------------------------------- | :------------ | -| pcl_grid_map_extraction/cluster_extraction/cluster_tolerance | float | Distance between points below which they will still be considered part of one cluster. | 0.2 | -| pcl_grid_map_extraction/cluster_extraction/min_num_points | int | Min number of points that a cluster needs to have (otherwise it will be discarded). | 3 | -| pcl_grid_map_extraction/cluster_extraction/max_num_points | int | Max number of points that a cluster can have (otherwise it will be discarded). | 1000000 | - -##### Outlier removal parameters - -See for more explanation on outlier removal. - -| Name | Type | Description | Default value | -| :--------------------------------------------------------- | :---- | :----------------------------------------------------------------------------- | :------------ | -| pcl_grid_map_extraction/outlier_removal/is_remove_outliers | float | Whether to perform statistical outlier removal. | false | -| pcl_grid_map_extraction/outlier_removal/mean_K | float | Number of neighbours to analyze for estimating statistics of a point. | 10 | -| pcl_grid_map_extraction/outlier_removal/stddev_threshold | float | Number of standard deviations under which points are considered to be inliers. | 1.0 | - -##### Subsampling parameters - -See for more explanation on point cloud downsampling. - -| Name | Type | Description | Default value | -| :------------------------------------------------------- | :---- | :-------------------------------------- | :------------ | -| pcl_grid_map_extraction/downsampling/is_downsample_cloud | bool | Whether to perform downsampling or not. | false | -| pcl_grid_map_extraction/downsampling/voxel_size | float | Voxel sizes (xyz) in meters. | 0.02 | diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 750243a27cca0..c91e16847dff7 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -13,13 +13,8 @@ autoware_auto_mapping_msgs autoware_utils geometry_msgs - grid_map_cv - grid_map_pcl - grid_map_ros - hash_library_vendor lanelet2_extension libpcl-all-dev - nlohmann-json-dev pcl_conversions rclcpp rclcpp_components diff --git a/perception/elevation_map_loader/CMakeLists.txt b/perception/elevation_map_loader/CMakeLists.txt new file mode 100644 index 0000000000000..8f5faa9a38513 --- /dev/null +++ b/perception/elevation_map_loader/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(elevation_map_loader) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(PCL REQUIRED COMPONENTS io) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(elevation_map_loader_node SHARED + src/elevation_map_loader_node.cpp +) +target_link_libraries(elevation_map_loader_node ${PCL_LIBRARIES}) + +rclcpp_components_register_node(elevation_map_loader_node + PLUGIN "ElevationMapLoaderNode" + EXECUTABLE elevation_map_loader +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config + data +) diff --git a/perception/elevation_map_loader/README.md b/perception/elevation_map_loader/README.md new file mode 100644 index 0000000000000..aa7f803a82dcf --- /dev/null +++ b/perception/elevation_map_loader/README.md @@ -0,0 +1,114 @@ +# elevation_map_loader package + +This package provides elevation map for compare_map_segmentation. + +## elevation_map_loader + +### Feature + +Generate elevation_map from subscribed pointcloud_map and vector_map and publish it. +Save the generated elevation_map locally and load it from next time. + +The elevation value of each cell is the average value of z of the points of the lowest cluster. +Cells with No elevation value can be inpainted using the values of neighboring cells. + +

+ +

+ +### How to run + +`ros2 run elevation_map_loader elevation_map_loader --ros-args -p param_file_path:=path/to/elevation_map_parameters.yaml -p elevation_map_directory:=path/to/elevation_map_directory -p pointcloud_map_path:=path/to/pointcloud.pcd` + +### Subscribed Topics + +- input/pointcloud_map (sensor_msgs:PointCloud2) : PointCloud Map +- input/vector_map (autoware_auto_mapping_msgs/HADMapBin) : binary data of Lanelet2 Map + +### Published Topics + +- output/elevation_map (grid_map_msgs/GridMap) : Elevation Map +- output/elevation_map_cloud (sensor_msgs:PointCloud2) : Pointcloud generated from the value of Elevation Map + +### Parameter description + +#### ROS parameters + +| Name | Type | Description | Default value | +| :-------------------------------- | :---------- | :--------------------------------------------------------------------------------------------------------- | :------------ | +| map_layer_name | std::string | elevation_map layer name | elevation | +| param_file_path | std::string | GridMap parameters config | path_default | +| elevation_map_file_path | std::string | elevation_map file (bag2) | path_default | +| map_frame | std::string | map_frame when loading elevation_map file | map | +| use_inpaint | bool | Whether to inpaint empty cells | true | +| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | +| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | +| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | +| lane_margin | float | Value of how much to expand the range of vector_map [m] | 0.5 | +| lane_height_diff_thresh | float | Only point clouds in the height range of this value from vector_map are used to generate elevation_map [m] | 1.0 | +| lane_filter_voxel_size_x | float | Voxel size x for calculating point clouds in vector_map [m] | 0.04 | +| lane_filter_voxel_size_y | float | Voxel size y for calculating point clouds in vector_map [m] | 0.04 | +| lane_filter_voxel_size_z | float | Voxel size z for calculating point clouds in vector_map [m] | 0.04 | + +#### GridMap parameters + +The parameters are described on `config/elevation_map_parameters.yaml`. + +##### General parameters + +| Name | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----------------------------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/num_processing_threads | int | Number of threads for processing grid map cells. Filtering of the raw input point cloud is not parallelized. | 12 | + +##### Grid map parameters + +See: + +Resulting grid map parameters. + +| Name | Type | Description | Default value | +| :------------------------------------------------------- | :---- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/grid_map/min_num_points_per_cell | int | Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN. | 3 | +| pcl_grid_map_extraction/grid_map/resolution | float | Resolution of the grid map. Width and length are computed automatically. | 0.3 | +| pcl_grid_map_extraction/grid_map/height_type | int | The parameter that determine the elevation of a cell `0: Smallest value among the average values of each cluster`, `1: Mean value of the cluster with the most points` | 1 | +| pcl_grid_map_extraction/grid_map/height_thresh | float | Height range from the smallest cluster (Only for height_type 1) | 1.0 | + +#### Point Cloud Pre-processing Parameters + +##### Rigid body transform parameters + +Rigid body transform that is applied to the point cloud before computing elevation. + +| Name | Type | Description | Default value | +| :-------------------------------------------------- | :---- | :---------------------------------------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/cloud_transform/translation | float | Translation (xyz) that is applied to the input point cloud before computing elevation. | 0.0 | +| pcl_grid_map_extraction/cloud_transform/rotation | float | Rotation (intrinsic rotation, convention X-Y'-Z'') that is applied to the input point cloud before computing elevation. | 0.0 | + +##### Cluster extraction parameters + +Cluster extraction is based on pcl algorithms. See for more details. + +| Name | Type | Description | Default value | +| :----------------------------------------------------------- | :---- | :------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/cluster_extraction/cluster_tolerance | float | Distance between points below which they will still be considered part of one cluster. | 0.2 | +| pcl_grid_map_extraction/cluster_extraction/min_num_points | int | Min number of points that a cluster needs to have (otherwise it will be discarded). | 3 | +| pcl_grid_map_extraction/cluster_extraction/max_num_points | int | Max number of points that a cluster can have (otherwise it will be discarded). | 1000000 | + +##### Outlier removal parameters + +See for more explanation on outlier removal. + +| Name | Type | Description | Default value | +| :--------------------------------------------------------- | :---- | :----------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/outlier_removal/is_remove_outliers | float | Whether to perform statistical outlier removal. | false | +| pcl_grid_map_extraction/outlier_removal/mean_K | float | Number of neighbours to analyze for estimating statistics of a point. | 10 | +| pcl_grid_map_extraction/outlier_removal/stddev_threshold | float | Number of standard deviations under which points are considered to be inliers. | 1.0 | + +##### Subsampling parameters + +See for more explanation on point cloud downsampling. + +| Name | Type | Description | Default value | +| :------------------------------------------------------- | :---- | :-------------------------------------- | :------------ | +| pcl_grid_map_extraction/downsampling/is_downsample_cloud | bool | Whether to perform downsampling or not. | false | +| pcl_grid_map_extraction/downsampling/voxel_size | float | Voxel sizes (xyz) in meters. | 0.02 | diff --git a/map/map_loader/config/elevation_map_parameters.yaml b/perception/elevation_map_loader/config/elevation_map_parameters.yaml similarity index 93% rename from map/map_loader/config/elevation_map_parameters.yaml rename to perception/elevation_map_loader/config/elevation_map_parameters.yaml index 51edc3ffd9f16..955a9e197ea0b 100644 --- a/map/map_loader/config/elevation_map_parameters.yaml +++ b/perception/elevation_map_loader/config/elevation_map_parameters.yaml @@ -26,3 +26,5 @@ pcl_grid_map_extraction: grid_map: min_num_points_per_cell: 3 resolution: 0.3 + height_type: 1 + height_thresh: 1.0 diff --git a/map/map_loader/data/elevation_maps/.empty b/perception/elevation_map_loader/data/elevation_maps/.empty similarity index 100% rename from map/map_loader/data/elevation_maps/.empty rename to perception/elevation_map_loader/data/elevation_maps/.empty diff --git a/map/map_loader/include/map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp similarity index 80% rename from map/map_loader/include/map_loader/elevation_map_loader_node.hpp rename to perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index bc1e34fe81136..f497307b43d21 100644 --- a/map/map_loader/include/map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -22,9 +22,9 @@ #include #include #include -#include #include +#include "autoware_external_api_msgs/msg/map_hash.hpp" #include #include @@ -32,9 +32,29 @@ #include #include +#include #include #include +class DataManager +{ +public: + DataManager() = default; + bool isInitialized() + { + if (use_lane_filter_) { + return static_cast(elevation_map_path_) && static_cast(map_pcl_ptr_) && + static_cast(lanelet_map_ptr_); + } else { + return static_cast(elevation_map_path_) && static_cast(map_pcl_ptr_); + } + } + std::unique_ptr elevation_map_path_; + pcl::PointCloud::Ptr map_pcl_ptr_; + lanelet::LaneletMapPtr lanelet_map_ptr_; + bool use_lane_filter_ = false; +}; + class ElevationMapLoaderNode : public rclcpp::Node { public: @@ -43,10 +63,15 @@ class ElevationMapLoaderNode : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_pointcloud_map_; rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_map_hash_; rclcpp::Publisher::SharedPtr pub_elevation_map_; rclcpp::Publisher::SharedPtr pub_elevation_map_cloud_; void onPointcloudMap(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map); + void onMapHash(const autoware_external_api_msgs::msg::MapHash::SharedPtr map_hash); void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map); + + void publish(); + void createElevationMap(); void setVerbosityLevelToDebugIfFlagSet(); void createElevationMapFromPointcloud(); autoware_utils::LinearRing2d getConvexHull( @@ -59,27 +84,21 @@ class ElevationMapLoaderNode : public rclcpp::Node const pcl::PointCloud::Ptr & cloud); bool checkPointWithinLanelets( const pcl::PointXYZ & point, const lanelet::ConstLanelets & joint_lanelets); - void publishElevationMap(); void inpaintElevationMap(const float radius); pcl::PointCloud::Ptr createPointcloudFromElevationMap(); void saveElevationMap(); float calculateDistancePointFromPlane( const pcl::PointXYZ & point, const lanelet::ConstLanelet & lanelet); - pcl::PointCloud::Ptr map_pcl_ptr_; grid_map::GridMap elevation_map_; std::string layer_name_; - std::filesystem::path elevation_map_path_; - nlohmann::json hash_json_; std::string map_frame_; bool use_inpaint_; float inpaint_radius_; - bool already_sub_vector_map_; - bool already_sub_pointcloud_map_; - bool use_elevation_map_file_; bool use_elevation_map_cloud_publisher_; pcl::shared_ptr grid_map_pcl_loader_; + DataManager data_manager_; struct LaneFilter { float voxel_size_x_; diff --git a/map/map_loader/launch/elevation_map_loader.launch.xml b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml similarity index 77% rename from map/map_loader/launch/elevation_map_loader.launch.xml rename to perception/elevation_map_loader/launch/elevation_map_loader.launch.xml index 09c73cfcc4ff2..9060db1febc94 100644 --- a/map/map_loader/launch/elevation_map_loader.launch.xml +++ b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml @@ -1,7 +1,6 @@ - - - + + @@ -13,12 +12,11 @@ - + - diff --git a/map/map_loader/media/elevation_map.png b/perception/elevation_map_loader/media/elevation_map.png similarity index 100% rename from map/map_loader/media/elevation_map.png rename to perception/elevation_map_loader/media/elevation_map.png diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml new file mode 100644 index 0000000000000..f3eab717c6b2d --- /dev/null +++ b/perception/elevation_map_loader/package.xml @@ -0,0 +1,33 @@ + + + elevation_map_loader + 0.1.0 + The map_loader package + Kosuke Takeuchi + Taichi Higashide + Apache License 2.0 + + ament_cmake + ament_cmake_auto + + autoware_auto_mapping_msgs + autoware_external_api_msgs + autoware_utils + grid_map_cv + grid_map_pcl + grid_map_ros + lanelet2_extension + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/map/map_loader/src/elevation_map_loader/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp similarity index 71% rename from map/map_loader/src/elevation_map_loader/elevation_map_loader_node.cpp rename to perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 106d12c502c52..2a8773cd7be1d 100644 --- a/map/map_loader/src/elevation_map_loader/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_loader/elevation_map_loader_node.hpp" +#include "elevation_map_loader/elevation_map_loader_node.hpp" #include #include #include #include #include +#include #include @@ -26,7 +27,6 @@ #include #include -#include #include #include #include @@ -45,72 +45,6 @@ #include #include -namespace -{ -bool isPcdFile(const std::string & p) -{ - if (!std::filesystem::is_regular_file(std::filesystem::status(p))) { - return false; - } - - const auto ext = p.substr(p.find_last_of(".") + 1); - - if (ext != "pcd" && ext != "PCD") { - return false; - } - - return true; -} - -std::string getMd5Sum(const std::string & data) -{ - MD5 digest_md5; - digest_md5.add(data.c_str(), data.size()); - return digest_md5.getHash(); -} - -// reference https://create.stephan-brumme.com/hash-library/ -std::string getMd5Sum(const std::filesystem::path & file_path) -{ - MD5 digest_md5; - const size_t buffer_size = 1'000'000; - auto buffer = std::array(); - std::ifstream file(file_path, std::ios::in | std::ios::binary); - - while (file) { - file.read(buffer.begin(), buffer_size); - const auto num_bytes_read = static_cast(file.gcount()); - digest_md5.add(buffer.begin(), num_bytes_read); - } - - return digest_md5.getHash(); -} - -nlohmann::json getPcdMapHashJson(const std::string & file_path) -{ - // Get PCD file paths - std::vector pcd_file_paths; - if (std::filesystem::is_directory(file_path)) { - for (const auto & file : std::filesystem::directory_iterator(file_path)) { - if (!isPcdFile(file.path())) { - continue; - } - pcd_file_paths.push_back(file.path()); - } - } else if (isPcdFile(file_path)) { - pcd_file_paths.push_back(file_path); - } - - // Create JSON meta file - nlohmann::json j; - for (const auto & file_path : pcd_file_paths) { - j[file_path.string()] = getMd5Sum(file_path); - } - - return j; -} -} // namespace - ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & options) : Node("elevation_map_loader", options) { @@ -122,14 +56,19 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio use_elevation_map_cloud_publisher_ = this->declare_parameter("use_elevation_map_cloud_publisher", false); - lane_filter_.use_lane_filter_ = this->declare_parameter("use_lane_filter", false); + const bool use_lane_filter = this->declare_parameter("use_lane_filter", false); + data_manager_.use_lane_filter_ = use_lane_filter; + + lane_filter_.use_lane_filter_ = use_lane_filter; lane_filter_.lane_margin_ = this->declare_parameter("lane_margin", 0.5); lane_filter_.lane_height_diff_thresh_ = this->declare_parameter("lane_height_diff_thresh", 1.0); lane_filter_.voxel_size_x_ = declare_parameter("lane_filter_voxel_size_x", 0.04); 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); - grid_map_pcl_loader_ = pcl::make_shared(this->get_logger()); + 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}; @@ -142,32 +81,55 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio "output/elevation_map_cloud", durable_qos); } - std::filesystem::path pointcloud_map_path( - this->declare_parameter("pointcloud_map_path", "path_default")); - hash_json_ = getPcdMapHashJson(pointcloud_map_path.lexically_normal().string()); - - const auto elevation_map_hash = getMd5Sum(hash_json_.dump()); - const std::string elevation_map_directory = - this->declare_parameter("elevation_map_directory", "path_default"); - elevation_map_path_ = std::filesystem::path(elevation_map_directory) / elevation_map_hash; + using std::placeholders::_1; + sub_map_hash_ = create_subscription( + "/api/autoware/get/map/info/hash", durable_qos, + std::bind(&ElevationMapLoaderNode::onMapHash, this, _1)); + sub_pointcloud_map_ = this->create_subscription( + "input/pointcloud_map", durable_qos, + std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1)); + sub_vector_map_ = this->create_subscription( + "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1)); +} - use_elevation_map_file_ = false; +void ElevationMapLoaderNode::publish() +{ struct stat info; - if (stat(elevation_map_path_.c_str(), &info) != 0) { + if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) { RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map "); - already_sub_vector_map_ = false; - already_sub_pointcloud_map_ = false; - using std::placeholders::_1; - sub_pointcloud_map_ = this->create_subscription( - "input/pointcloud_map", durable_qos, - std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1)); - sub_vector_map_ = this->create_subscription( - "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1)); + createElevationMap(); } else if (info.st_mode & S_IFDIR) { - RCLCPP_INFO(this->get_logger(), "Load elevation map from: %s", elevation_map_path_.c_str()); - use_elevation_map_file_ = grid_map::GridMapRosConverter::loadFromBag( - elevation_map_path_, "elevation_map", elevation_map_); - publishElevationMap(); + RCLCPP_INFO( + this->get_logger(), "Load elevation map from: %s", + data_manager_.elevation_map_path_->c_str()); + grid_map::GridMapRosConverter::loadFromBag( + *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); + } + + elevation_map_.setFrameId(map_frame_); + auto msg = grid_map::GridMapRosConverter::toMessage(elevation_map_); + pub_elevation_map_->publish(std::move(msg)); + + if (use_elevation_map_cloud_publisher_) { + pcl::PointCloud::Ptr elevation_map_cloud_ptr = + createPointcloudFromElevationMap(); + sensor_msgs::msg::PointCloud2 elevation_map_cloud_msg; + pcl::toROSMsg(*elevation_map_cloud_ptr, elevation_map_cloud_msg); + pub_elevation_map_cloud_->publish(elevation_map_cloud_msg); + } +} + +void ElevationMapLoaderNode::onMapHash( + const autoware_external_api_msgs::msg::MapHash::SharedPtr map_hash) +{ + RCLCPP_INFO(this->get_logger(), "subscribe map_hash"); + const auto elevation_map_hash = map_hash->pcd; + const std::string elevation_map_directory = + this->declare_parameter("elevation_map_directory", "path_default"); + data_manager_.elevation_map_path_ = std::make_unique( + std::filesystem::path(elevation_map_directory) / elevation_map_hash); + if (data_manager_.isInitialized()) { + publish(); } } @@ -177,11 +139,9 @@ void ElevationMapLoaderNode::onPointcloudMap( RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map"); pcl::PointCloud map_pcl; pcl::fromROSMsg(*pointcloud_map, map_pcl); - map_pcl_ptr_ = pcl::make_shared>(map_pcl); - - already_sub_pointcloud_map_ = true; - if (already_sub_vector_map_) { - publishElevationMap(); + data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + if (data_manager_.isInitialized()) { + publish(); } } @@ -189,56 +149,34 @@ void ElevationMapLoaderNode::onVectorMap( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map) { RCLCPP_INFO(this->get_logger(), "subscribe vector_map"); - already_sub_vector_map_ = false; - lanelet::LaneletMapPtr lanelet_map_ptr; - lanelet_map_ptr = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*vector_map, lanelet_map_ptr); - const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + data_manager_.lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*vector_map, data_manager_.lanelet_map_ptr_); + const lanelet::ConstLanelets all_lanelets = + lanelet::utils::query::laneletLayer(data_manager_.lanelet_map_ptr_); lane_filter_.road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); - - already_sub_vector_map_ = true; - if (already_sub_pointcloud_map_) { - publishElevationMap(); + if (data_manager_.isInitialized()) { + publish(); } } -void ElevationMapLoaderNode::publishElevationMap() +void ElevationMapLoaderNode::createElevationMap() { - if (!use_elevation_map_file_) { - if (lane_filter_.use_lane_filter_) { - // calculate convex hull - const auto convex_hull = getConvexHull(map_pcl_ptr_); - // get intersected lanelets - lanelet::ConstLanelets intersected_lanelets = - getIntersectedLanelets(convex_hull, lane_filter_.road_lanelets_); - // filter pointcloud by lanelet - pcl::PointCloud::Ptr lane_filtered_map_pcl_ptr = - getLaneFilteredPointCloud(intersected_lanelets, map_pcl_ptr_); - grid_map_pcl_loader_->setInputCloud(lane_filtered_map_pcl_ptr); - } else { - grid_map_pcl_loader_->setInputCloud(map_pcl_ptr_); - } - createElevationMapFromPointcloud(); - elevation_map_ = grid_map_pcl_loader_->getGridMap(); - if (use_inpaint_) { - inpaintElevationMap(inpaint_radius_); - } - saveElevationMap(); + 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); + } else { + grid_map_pcl_loader_->setInputCloud(data_manager_.map_pcl_ptr_); } - - elevation_map_.setFrameId(map_frame_); - auto msg = grid_map::GridMapRosConverter::toMessage(elevation_map_); - pub_elevation_map_->publish(std::move(msg)); - already_sub_pointcloud_map_ = false; - already_sub_vector_map_ = false; - - if (use_elevation_map_cloud_publisher_) { - pcl::PointCloud::Ptr elevation_map_cloud_ptr = - createPointcloudFromElevationMap(); - sensor_msgs::msg::PointCloud2 elevation_map_cloud_msg; - pcl::toROSMsg(*elevation_map_cloud_ptr, elevation_map_cloud_msg); - pub_elevation_map_cloud_->publish(elevation_map_cloud_msg); + createElevationMapFromPointcloud(); + elevation_map_ = grid_map_pcl_loader_->getGridMap(); + if (use_inpaint_) { + inpaintElevationMap(inpaint_radius_); } + saveElevationMap(); } void ElevationMapLoaderNode::createElevationMapFromPointcloud() @@ -438,13 +376,8 @@ pcl::PointCloud::Ptr ElevationMapLoaderNode::createPointcloudFrom void ElevationMapLoaderNode::saveElevationMap() { - const bool saving_successful = - grid_map::GridMapRosConverter::saveToBag(elevation_map_, elevation_map_path_, "elevation_map"); - - std::ofstream json_file(elevation_map_path_ / "input_pcd.json"); - json_file << hash_json_; - json_file.close(); - + const bool saving_successful = grid_map::GridMapRosConverter::saveToBag( + elevation_map_, *data_manager_.elevation_map_path_, "elevation_map"); RCLCPP_INFO_STREAM( this->get_logger(), "Saving elevation map successful: " << std::boolalpha << saving_successful); }