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);
}