diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 867eb99b8f8b3..2624b4244ec8e 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -89,6 +89,7 @@ def launch_setup(context, *args, **kwargs): ], parameters=[ {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, + {"pcd_metadata_path": [LaunchConfiguration("pointcloud_map_metadata_path")]}, pointcloud_map_loader_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -151,6 +152,11 @@ def add_launch_arg(name: str, default_value=None, description=None): [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"], "path to pointcloud map file", ), + add_launch_arg( + "pointcloud_map_metadata_path", + [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], + "path to pointcloud map metadata file", + ), add_launch_arg( "lanelet2_map_loader_param_path", [ diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index b8abb4da84e4a..9cedb28edf539 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) +target_link_libraries(pointcloud_map_loader_node yaml-cpp) target_include_directories(pointcloud_map_loader_node SYSTEM PUBLIC @@ -59,12 +60,15 @@ if(BUILD_TESTING) ament_add_gmock(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) target_link_libraries(${test_name} ${${PROJECT_NAME}_LIBRARIES}) + target_link_libraries(${test_name} yaml-cpp) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_sphere_box_overlap.cpp) + add_testcase(test/test_replace_with_absolute_path.cpp) + add_testcase(test/test_load_pcd_metadata.cpp) add_testcase(test/test_pointcloud_map_loader_module.cpp) add_testcase(test/test_partial_map_loader_module.cpp) add_testcase(test/test_differential_map_loader_module.cpp) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index c8f5bed19bb3d..80d42e727dff6 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -38,13 +38,15 @@ Please see [the description of `GetDifferentialPointCloudMap.srv`](https://githu ### Parameters -| 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 | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | -| leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | +| 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 | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | +| leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | +| pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | +| pcd_metadata_path | std::string | Path to pointcloud metadata file | | ### Interfaces @@ -52,6 +54,48 @@ Please see [the description of `GetDifferentialPointCloudMap.srv`](https://githu - `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 - `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map +- pointcloud map file(s) (.pcd) +- metadata of pointcloud map(s) (.yaml) + +### Metadata + +You must provide metadata in YAML format as well as pointcloud map files. Pointcloud map should be divided into one or more files with x-y grid. + +Metadata should look like this: + +```yaml +x_resolution: 100.0 +y_resolution: 150.0 +A.pcd: [1200, 2500] # -> 1200 < x < 1300, 2500 < y < 2650 +B.pcd: [1300, 2500] # -> 1300 < x < 1400, 2500 < y < 2650 +C.pcd: [1200, 2650] # -> 1200 < x < 1300, 2650 < y < 2800 +D.pcd: [1400, 2650] # -> 1400 < x < 1500, 2650 < y < 2800 +``` + +You may use [pointcloud_divider](https://github.com/MapIV/pointcloud_divider) from MAP IV for dividing pointcloud map as well as generating the compatible metadata.yaml. + +### How to store map-related files + +If you only have one pointcloud map, Autoware will assume the following directory structure by default. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +``` + +If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map +├── pcd_00.pcd +├── pcd_01.pcd +├── pcd_02.pcd +├── ... +└── pointcloud_map_metadata.yaml +``` --- diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 3cfc457c15d22..df32fab0cda1c 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -1,11 +1,13 @@ + + diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 7a22a369744ab..4fa5cc4156794 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -29,6 +29,7 @@ tf2_ros tier4_autoware_utils visualization_msgs + yaml-cpp ament_cmake_gmock ament_lint_auto diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 8322ac5f60b99..1e81218c42410 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -48,6 +48,10 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } else { autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id); + pointcloud_map_cell_with_id.min_x = metadata.min.x; + pointcloud_map_cell_with_id.min_y = metadata.min.y; + pointcloud_map_cell_with_id.max_x = metadata.max.x; + pointcloud_map_cell_with_id.max_y = metadata.max.y; response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 1454b01147ac2..234f78c2a071e 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -29,7 +29,6 @@ void PartialMapLoaderModule::partialAreaLoad( GetPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids - for (const auto & ele : all_pcd_file_metadata_dict_) { std::string path = ele.first; PCDFileMetadata metadata = ele.second; @@ -42,6 +41,11 @@ void PartialMapLoaderModule::partialAreaLoad( autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id); + pointcloud_map_cell_with_id.min_x = metadata.min.x; + pointcloud_map_cell_with_id.min_y = metadata.min.y; + pointcloud_map_cell_with_id.max_x = metadata.max.x; + pointcloud_map_cell_with_id.max_y = metadata.max.y; + response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } 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 ecbe481345381..7feea26bcb073 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 @@ -49,6 +49,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt { const auto pcd_paths = getPcdPaths(declare_parameter>("pcd_paths_or_directory")); + std::string pcd_metadata_path = declare_parameter("pcd_metadata_path"); 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"); @@ -66,18 +67,52 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load | enable_differential_load) { - pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); - } + if (enable_partial_load || enable_differential_load) { + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } + + if (enable_differential_load) { + differential_map_loader_ = + std::make_unique(this, pcd_metadata_dict); + } } +} + +std::map PointCloudMapLoaderNode::getPCDMetadata( + const std::string & pcd_metadata_path, const std::vector & pcd_paths) const +{ + std::map pcd_metadata_dict; + if (pcd_paths.size() != 1) { + if (!fs::exists(pcd_metadata_path)) { + throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict_); + pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); + pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict_, pcd_paths); + RCLCPP_INFO_STREAM(get_logger(), "Loaded PCD metadata: " << pcd_metadata_path); + } else { + // An exception when using a single file PCD map so that the users do not have to provide + // a metadata file. + // Note that this should ideally be avoided and thus eventually be removed by someone, until + // Autoware users get used to handling the PCD file(s) with metadata. + RCLCPP_INFO_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file."); + pcl::PointCloud single_pcd; + if (pcl::io::loadPCDFile(pcd_paths[0], single_pcd) == -1) { + throw std::runtime_error("PCD load failed: " + pcd_paths[0]); + } + PCDFileMetadata metadata = {}; + pcl::getMinMax3D(single_pcd, metadata.min, metadata.max); + pcd_metadata_dict[pcd_paths[0]] = metadata; } + return pcd_metadata_dict; } std::vector PointCloudMapLoaderNode::getPcdPaths( @@ -105,21 +140,5 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( return pcd_paths; } -std::map PointCloudMapLoaderNode::generatePCDMetadata( - const std::vector & pcd_paths) const -{ - pcl::PointCloud partial_pcd; - std::map all_pcd_file_metadata_dict; - for (const auto & path : pcd_paths) { - if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { - RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path); - } - PCDFileMetadata metadata = {}; - pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max); - all_pcd_file_metadata_dict[path] = metadata; - } - return all_pcd_file_metadata_dict; -} - #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) 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 d321902131a81..27c90080b7f9a 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 @@ -48,8 +48,8 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; - std::map generatePCDMetadata( - const std::vector & pcd_paths) const; + std::map getPCDMetadata( + const std::string & pcd_metadata_path, const std::vector & pcd_paths) const; }; #endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 8af8c0dfd8ec0..62a3f064d18e0 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -16,6 +16,53 @@ #include +#include +#include +#include + +std::map loadPCDMetadata(const std::string & pcd_metadata_path) +{ + YAML::Node config = YAML::LoadFile(pcd_metadata_path); + + std::map metadata; + + for (const auto & node : config) { + if ( + node.first.as() == "x_resolution" || + node.first.as() == "y_resolution") { + continue; + } + + std::string key = node.first.as(); + std::vector values = node.second.as>(); + + PCDFileMetadata fileMetadata; + fileMetadata.min.x = values[0]; + fileMetadata.min.y = values[1]; + fileMetadata.max.x = values[0] + config["x_resolution"].as(); + fileMetadata.max.y = values[1] + config["y_resolution"].as(); + + metadata[key] = fileMetadata; + } + + return metadata; +} + +std::map replaceWithAbsolutePath( + const std::map & pcd_metadata_path, + const std::vector & pcd_paths) +{ + std::map absolute_path_map; + for (const auto & path : pcd_paths) { + std::string filename = path.substr(path.find_last_of("/\\") + 1); + auto it = pcd_metadata_path.find(filename); + if (it != pcd_metadata_path.end()) { + absolute_path_map[path] = it->second; + } + } + return absolute_path_map; +} + bool sphereAndBoxOverlapExists( const geometry_msgs::msg::Point center, const double radius, const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point) diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 0a0d2912bb8c5..bbf55b72a2348 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -19,13 +19,28 @@ #include #include +#include + +#include +#include +#include struct PCDFileMetadata { pcl::PointXYZ min; pcl::PointXYZ max; + bool operator==(const PCDFileMetadata & other) const + { + return min.x == other.min.x && min.y == other.min.y && min.z == other.min.z && + max.x == other.max.x && max.y == other.max.y && max.z == other.max.z; + } }; +std::map loadPCDMetadata(const std::string & pcd_metadata_path); +std::map replaceWithAbsolutePath( + const std::map & pcd_metadata_path, + const std::vector & pcd_paths); + bool sphereAndBoxOverlapExists( const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, const pcl::PointXYZ position_max); diff --git a/map/map_loader/test/test_load_pcd_metadata.cpp b/map/map_loader/test/test_load_pcd_metadata.cpp new file mode 100644 index 0000000000000..a832489b6db99 --- /dev/null +++ b/map/map_loader/test/test_load_pcd_metadata.cpp @@ -0,0 +1,55 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pointcloud_map_loader/utils.hpp" + +#include + +#include +#include + +using ::testing::ContainerEq; + +std::string createYAMLFile() +{ + std::filesystem::path tmp_path = std::filesystem::temp_directory_path() / "temp_metadata.yaml"; + + std::ofstream ofs(tmp_path); + ofs << "file1.pcd: [1, 2]\n"; + ofs << "file2.pcd: [3, 4]\n"; + ofs << "x_resolution: 5\n"; + ofs << "y_resolution: 6\n"; + ofs.close(); + + return tmp_path.string(); +} + +TEST(LoadPCDMetadataTest, BasicFunctionality) +{ + std::string yaml_file_path = createYAMLFile(); + + std::map expected = { + {"file1.pcd", {{1, 2, 0}, {6, 8, 0}}}, + {"file2.pcd", {{3, 4, 0}, {8, 10, 0}}}, + }; + + auto result = loadPCDMetadata(yaml_file_path); + ASSERT_THAT(result, ContainerEq(expected)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp index dce7e564956f1..72dd75d3b738e 100644 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -24,7 +24,6 @@ #include #include -#include #include using std::chrono_literals::operator""ms; @@ -57,13 +56,7 @@ class TestPointcloudMapLoaderModule : public ::testing::Test pcl::io::savePCDFileASCII(temp_pcd_path, cloud); } - void TearDown() override - { - // Delete the temporary PCD file - std::filesystem::remove(temp_pcd_path); - - rclcpp::shutdown(); - } + void TearDown() override { rclcpp::shutdown(); } }; TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/map/map_loader/test/test_replace_with_absolute_path.cpp new file mode 100644 index 0000000000000..10680e05103ce --- /dev/null +++ b/map/map_loader/test/test_replace_with_absolute_path.cpp @@ -0,0 +1,65 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pointcloud_map_loader/utils.hpp" + +#include +#include + +using ::testing::ContainerEq; + +TEST(ReplaceWithAbsolutePathTest, BasicFunctionality) +{ + std::map pcd_metadata_path = { + {"file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, + {"file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, + }; + + std::vector pcd_paths = { + "/home/user/pcd/file1.pcd", + "/home/user/pcd/file2.pcd", + }; + + std::map expected = { + {"/home/user/pcd/file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, + {"/home/user/pcd/file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, + }; + + auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); + ASSERT_THAT(result, ContainerEq(expected)); +} + +TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles) +{ + std::map pcd_metadata_path = { + {"file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, + {"file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, + }; + + std::vector pcd_paths = { + "/home/user/pcd/non_matching_file1.pcd", + "/home/user/pcd/non_matching_file2.pcd", + }; + + std::map expected = {}; + + auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); + ASSERT_THAT(result, ContainerEq(expected)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +}