From 75642376be49489024c5f5c3ae83ea46cd8270b8 Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Wed, 4 Sep 2024 16:41:16 +0700 Subject: [PATCH] feat(map): add pointcloud divider and pointcloud merger (#100) * Added point cloud merger Signed-off-by: Anh Nguyen * Update README and remove unused svg file Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Fix pre-commit Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Fix spell check Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Fix pre-commit ci Signed-off-by: Anh Nguyen * Fix pre-commit ci Signed-off-by: Anh Nguyen * Fix pre-commit ci Signed-off-by: Anh Nguyen * Add autoware_ prefix to nodes Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Add autoware_ prefix to pointcloud merger and divider; refactor code; rearrange files Signed-off-by: Anh Nguyen * Follow autoware directory structure Signed-off-by: Anh Nguyen * Move parameters to yaml files, create schema json files, and apply clang format Signed-off-by: Anh Nguyen * Fix an error while reading PCD file Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Fix pre-commit ci Signed-off-by: Anh Nguyen * Fix pre-commit-optional Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Fix merger's schema file Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Fixed review comments Signed-off-by: Anh Nguyen * style(pre-commit): autofix * Fixed spell check Signed-off-by: Anh Nguyen --------- Signed-off-by: Anh Nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yamato Ando --- .../CMakeLists.txt | 37 ++ map/autoware_pointcloud_divider/LICENSE | 28 + map/autoware_pointcloud_divider/README.md | 100 +++ .../config/pointcloud_divider.param.yaml | 10 + .../docs/output_file_name_pattern.drawio.svg | 32 + .../docs/pcd_divider.drawio.svg | 407 ++++++++++++ .../autoware/pointcloud_divider/centroid.hpp | 116 ++++ .../autoware/pointcloud_divider/grid_info.hpp | 113 ++++ .../pointcloud_divider/pcd_divider.hpp | 178 ++++++ .../autoware/pointcloud_divider/pcd_io.hpp | 21 + .../pointcloud_divider/pcd_io_reader.hpp | 603 ++++++++++++++++++ .../pointcloud_divider/pcd_io_writer.hpp | 458 +++++++++++++ .../autoware/pointcloud_divider/utility.hpp | 133 ++++ .../pointcloud_divider/voxel_grid_filter.hpp | 53 ++ .../launch/pointcloud_divider.launch.py | 85 +++ .../launch/pointcloud_divider.launch.xml | 9 + map/autoware_pointcloud_divider/package.xml | 25 + .../schema/pointcloud_divider.schema.json | 68 ++ .../src/include/pointcloud_divider_node.hpp | 34 + .../src/pcd_divider.cpp | 477 ++++++++++++++ .../src/pointcloud_divider_node.cpp | 95 +++ .../src/voxel_grid_filter.cpp | 47 ++ map/autoware_pointcloud_merger/CMakeLists.txt | 37 ++ map/autoware_pointcloud_merger/LICENSE | 28 + map/autoware_pointcloud_merger/README.md | 48 ++ .../config/pointcloud_merger.param.yaml | 7 + .../autoware/pointcloud_merger/pcd_merger.hpp | 108 ++++ .../launch/pointcloud_merger.launch.py | 81 +++ .../launch/pointcloud_merger.launch.xml | 9 + map/autoware_pointcloud_merger/package.xml | 22 + .../schema/pointcloud_merger.schema.json | 48 ++ .../src/include/pointcloud_merger_node.hpp | 34 + .../src/pcd_merger.cpp | 219 +++++++ .../src/pointcloud_merger_node.cpp | 78 +++ 34 files changed, 3848 insertions(+) create mode 100644 map/autoware_pointcloud_divider/CMakeLists.txt create mode 100644 map/autoware_pointcloud_divider/LICENSE create mode 100644 map/autoware_pointcloud_divider/README.md create mode 100644 map/autoware_pointcloud_divider/config/pointcloud_divider.param.yaml create mode 100644 map/autoware_pointcloud_divider/docs/output_file_name_pattern.drawio.svg create mode 100644 map/autoware_pointcloud_divider/docs/pcd_divider.drawio.svg create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/centroid.hpp create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/grid_info.hpp create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_divider.hpp create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io.hpp create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_reader.hpp create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_writer.hpp create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/utility.hpp create mode 100644 map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/voxel_grid_filter.hpp create mode 100644 map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.py create mode 100644 map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.xml create mode 100644 map/autoware_pointcloud_divider/package.xml create mode 100644 map/autoware_pointcloud_divider/schema/pointcloud_divider.schema.json create mode 100644 map/autoware_pointcloud_divider/src/include/pointcloud_divider_node.hpp create mode 100644 map/autoware_pointcloud_divider/src/pcd_divider.cpp create mode 100644 map/autoware_pointcloud_divider/src/pointcloud_divider_node.cpp create mode 100644 map/autoware_pointcloud_divider/src/voxel_grid_filter.cpp create mode 100644 map/autoware_pointcloud_merger/CMakeLists.txt create mode 100644 map/autoware_pointcloud_merger/LICENSE create mode 100644 map/autoware_pointcloud_merger/README.md create mode 100644 map/autoware_pointcloud_merger/config/pointcloud_merger.param.yaml create mode 100644 map/autoware_pointcloud_merger/include/autoware/pointcloud_merger/pcd_merger.hpp create mode 100644 map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.py create mode 100644 map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.xml create mode 100644 map/autoware_pointcloud_merger/package.xml create mode 100644 map/autoware_pointcloud_merger/schema/pointcloud_merger.schema.json create mode 100644 map/autoware_pointcloud_merger/src/include/pointcloud_merger_node.hpp create mode 100644 map/autoware_pointcloud_merger/src/pcd_merger.cpp create mode 100644 map/autoware_pointcloud_merger/src/pointcloud_merger_node.cpp diff --git a/map/autoware_pointcloud_divider/CMakeLists.txt b/map/autoware_pointcloud_divider/CMakeLists.txt new file mode 100644 index 00000000..4e0d8286 --- /dev/null +++ b/map/autoware_pointcloud_divider/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_pointcloud_divider) + +find_package(autoware_cmake REQUIRED) +autoware_package() +ament_auto_find_build_dependencies() + +# Enable support for C++17 +if (${CMAKE_VERSION} VERSION_LESS "3.1.0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") +else () + set(CMAKE_CXX_STANDARD 17) +endif () + +# Find packages +find_package(yaml-cpp REQUIRED) +find_package(PCL REQUIRED COMPONENTS common io filters) + +include_directories(include) + +# Add divider library +ament_auto_add_library(${PROJECT_NAME} SHARED src/pointcloud_divider_node.cpp src/voxel_grid_filter.cpp src/pcd_divider.cpp) +target_link_libraries(${PROJECT_NAME} yaml-cpp ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::pointcloud_divider::PointCloudDivider" + EXECUTABLE ${PROJECT_NAME}_node +) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include + ) + +ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/map/autoware_pointcloud_divider/LICENSE b/map/autoware_pointcloud_divider/LICENSE new file mode 100644 index 00000000..da934de9 --- /dev/null +++ b/map/autoware_pointcloud_divider/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2023, MAP IV + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/map/autoware_pointcloud_divider/README.md b/map/autoware_pointcloud_divider/README.md new file mode 100644 index 00000000..e859ad8b --- /dev/null +++ b/map/autoware_pointcloud_divider/README.md @@ -0,0 +1,100 @@ +# autoware_pointcloud_divider + +This is a tool for processing pcd files, and it can perform the following functions: + +- Dividing point clouds +- Downsampling point clouds +- Generating metadata to efficiently handle the divided point clouds + +## Supported Data Format + +**Currently, only `pcl::PointXYZ` and `pcl::PointXYZI` are supported. Any PCD will be loaded as those two types.** + +This tool can be used with files that have data fields other than `XYZI` (e.g., `XYZRGB`) and files that only contain `XYZ`. + +- Data fields other than `XYZI` are ignored during loading. +- When loading `XYZ`-only data, the `intensity` field is assigned 0. + +## Installation + +```bash +cd # OR +cd src/ +git clone git@github.com:autowarefoundation/autoware_tools.git +cd .. +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests --symlink-install --packages-up-to autoware_pointcloud_divider +``` + +## Usage + +- Select directory, process all files found with `find $INPUT_DIR -name "*.pcd"`. + + ```bash + ros2 launch autoware_pointcloud_divider pointcloud_divider.launch.xml input_pcd_or_dir:= output_pcd_dir:= prefix:= + ``` + + | Name | Description | + | ---------- | ------------------------------------- | + | INPUT_DIR | Directory that contains all PCD files | + | OUTPUT_DIR | Output directory name | + | PREFIX | Prefix of output PCD file name | + +`INPUT_DIR` and `OUTPUT_DIR` should be specified as **absolute paths**. + +NOTE: The folder `OUTPUT_DIR` is auto generated. If it already exists, all files within that folder will be deleted before the tool runs. Hence, users should backup the important files in that folder if necessary. + +### Parameters + +{{ json_to_markdown("map/autoware_pointcloud_divider/schema/pointcloud_divider.schema.json") }} + +How the point cloud is processed. + +![node_diagram](docs/pcd_divider.drawio.svg) + +How the PCD file is named + +![node_diagram](docs/output_file_name_pattern.drawio.svg) + +### Parameter example + +1. Dividing point clouds without downsampling + + ```yaml + use_large_grid: false + leaf_size: -1.0 # any negative number + grid_size_x: 20 + grid_size_y: 20 + ``` + +2. Dividing and downsampling point clouds + + ```yaml + use_large_grid: false + leaf_size: 0.2 + grid_size_x: 20 + grid_size_y: 20 + ``` + +## Metadata YAML Format + +The metadata file should be named `metadata.yaml`. It contains the following fields: + +- `x_resolution`: The resolution along the X-axis. +- `y_resolution`: The resolution along the Y-axis. + +Additionally, the file contains entries for individual point cloud files (`.pcd` files) and their corresponding grid coordinates. The key is the file name, and the value is a list containing the X and Y coordinates of the lower-left corner of the grid cell associated with that file. The grid cell's boundaries can be calculated using the `x_resolution` and `y_resolution` values. + +For example: + +```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 +``` + +## LICENSE + +Parts of files grid_info.hpp, pcd_divider.hpp, and pcd_divider.cpp are copied from [MapIV's pointcloud_divider](https://github.com/MapIV/pointcloud_divider) and are under [BSD-3-Clauses](LICENSE) license. The remaining code are under [Apache License 2.0](../../LICENSE) diff --git a/map/autoware_pointcloud_divider/config/pointcloud_divider.param.yaml b/map/autoware_pointcloud_divider/config/pointcloud_divider.param.yaml new file mode 100644 index 00000000..8c98b808 --- /dev/null +++ b/map/autoware_pointcloud_divider/config/pointcloud_divider.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + use_large_grid: false + leaf_size: -0.1 + grid_size_x: 20.0 + grid_size_y: 20.0 + input_pcd_or_dir: $(var input_pcd_or_dir) # Path to the folder containing the input PCD files + output_pcd_dir: $(var output_pcd_dir) # Path to the folder containing the segmented PCD files + prefix: $(var prefix) # Prefix for the name of the output PCD files + point_type: "point_xyzi" diff --git a/map/autoware_pointcloud_divider/docs/output_file_name_pattern.drawio.svg b/map/autoware_pointcloud_divider/docs/output_file_name_pattern.drawio.svg new file mode 100644 index 00000000..cfd75020 --- /dev/null +++ b/map/autoware_pointcloud_divider/docs/output_file_name_pattern.drawio.svg @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + diff --git a/map/autoware_pointcloud_divider/docs/pcd_divider.drawio.svg b/map/autoware_pointcloud_divider/docs/pcd_divider.drawio.svg new file mode 100644 index 00000000..7d5e0c8b --- /dev/null +++ b/map/autoware_pointcloud_divider/docs/pcd_divider.drawio.svg @@ -0,0 +1,407 @@ + + + + + + + + + + + + + + + + +
+
+
+ PCD_1 +
+
+
+ + PCD_1 + + + + + + + + + + + + +
+
+
+ PCD_2 +
+
+
+ + PCD_2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ grid_size_y +
+
+
+ + grid_size_y + + + + + + + + + + + + +
+
+
+ grid_size_x +
+
+
+ + grid_size_x + + + + + + + + + + + + + + + + + + + +
+
+
+ voxel downsampling is performed per each grid +
+
+
+ + voxel downsampling is performed per each grid + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/centroid.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/centroid.hpp new file mode 100644 index 00000000..c5c35165 --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/centroid.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__CENTROID_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__CENTROID_HPP_ + +#include "utility.hpp" + +#include + +#include + +namespace autoware::pointcloud_divider +{ + +template +void accumulate(const PointT & p, const PointT & first_p, PointT & acc_diff); +template +void compute_centroid( + const PointT & acc_diff, const PointT & first_p, size_t point_num, PointT & centroid); + +template <> +void accumulate(const pcl::PointXYZ & p, const pcl::PointXYZ & first_p, pcl::PointXYZ & acc_diff) +{ + acc_diff.x += p.x - first_p.x; + acc_diff.y += p.y - first_p.y; + acc_diff.z += p.z - first_p.z; +} + +template <> +void accumulate(const pcl::PointXYZI & p, const pcl::PointXYZI & first_p, pcl::PointXYZI & acc_diff) +{ + acc_diff.x += p.x - first_p.x; + acc_diff.y += p.y - first_p.y; + acc_diff.z += p.z - first_p.z; + acc_diff.intensity += p.intensity - first_p.intensity; +} + +template <> +void compute_centroid( + const pcl::PointXYZ & acc_diff, const pcl::PointXYZ & first_p, size_t point_num, + pcl::PointXYZ & centroid) +{ + double double_point_num = static_cast(point_num); + + centroid.x = acc_diff.x / double_point_num + first_p.x; + centroid.y = acc_diff.y / double_point_num + first_p.y; + centroid.z = acc_diff.z / double_point_num + first_p.z; +} + +template <> +void compute_centroid( + const pcl::PointXYZI & acc_diff, const pcl::PointXYZI & first_p, size_t point_num, + pcl::PointXYZI & centroid) +{ + double double_point_num = static_cast(point_num); + + centroid.x = acc_diff.x / double_point_num + first_p.x; + centroid.y = acc_diff.y / double_point_num + first_p.y; + centroid.z = acc_diff.z / double_point_num + first_p.z; + centroid.intensity = acc_diff.intensity / double_point_num + first_p.intensity; +} + +template +struct Centroid +{ + Centroid() + { + point_num_ = 0; + util::zero_point(acc_diff_); + util::zero_point(first_point_); + } + + void add(const PointT & p) + { + if (point_num_ == 0) { + first_point_ = p; + } else { + accumulate(p, first_point_, acc_diff_); + } + + ++point_num_; + } + + PointT get() + { + PointT centroid; + + if (point_num_ > 0) { + compute_centroid(acc_diff_, first_point_, point_num_, centroid); + } else { + std::cerr << "Error: there is no point in the centroid group!" << std::endl; + exit(EXIT_FAILURE); + } + + return centroid; + } + + PointT acc_diff_, first_point_; + size_t point_num_; +}; + +} // namespace autoware::pointcloud_divider + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__CENTROID_HPP_ diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/grid_info.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/grid_info.hpp new file mode 100644 index 00000000..452145f4 --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/grid_info.hpp @@ -0,0 +1,113 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__GRID_INFO_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__GRID_INFO_HPP_ + +#include +#include +#include + +namespace autoware::pointcloud_divider +{ + +template +struct GridInfo +{ + int ix, iy, iz; + + GridInfo() : ix(0), iy(0), iz(0) {} + + GridInfo(int x, int y, int z = 0) : ix(x), iy(y), iz(z) {} + + friend bool operator==(const GridInfo & one, const GridInfo & other) + { + return one.ix == other.ix && one.iy == other.iy && one.iz == other.iz; + } + + friend bool operator!=(const GridInfo & one, const GridInfo & other) { return !(one == other); } + + friend std::ostream & operator<<(std::ostream & os, const GridInfo & g) + { + if (dim < 3) { + os << g.ix << "_" << g.iy; + } else { + os << g.ix << "_" << g.iy << "_" << g.iz; + } + + return os; + } +}; + +} // namespace autoware::pointcloud_divider + +// This is for unordered_map and unordered_set +// For 2D voxel grid +namespace std +{ +template <> +struct hash> +{ +public: + size_t operator()(const autoware::pointcloud_divider::GridInfo<2> & grid) const + { + std::size_t seed = 0; + seed ^= std::hash{}(grid.ix) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + seed ^= std::hash{}(grid.iy) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + return seed; + } +}; + +// For 3D voxel grid +template <> +struct hash> +{ +public: + size_t operator()(const autoware::pointcloud_divider::GridInfo<3> & grid) const + { + std::size_t seed = 0; + + seed ^= std::hash{}(grid.ix) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + seed ^= std::hash{}(grid.iy) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + seed ^= std::hash{}(grid.iz) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + + return seed; + } +}; + +} // namespace std + +// Compute the index of the 2D voxel that contains a point +template +autoware::pointcloud_divider::GridInfo<2> pointToGrid2(const PointT & p, float res_x, float res_y) +{ + int x_id = static_cast(std::floor(p.x / res_x) * res_x); + int y_id = static_cast(std::floor(p.y / res_y) * res_y); + + return autoware::pointcloud_divider::GridInfo<2>(x_id, y_id); +} + +// Compute the index of the 3D voxel that contains a point +template +autoware::pointcloud_divider::GridInfo<3> pointToGrid3( + const PointT & p, float res_x, float res_y, float res_z) +{ + int x_id = static_cast(std::floor(p.x / res_x)); + int y_id = static_cast(std::floor(p.y / res_y)); + int z_id = static_cast(std::floor(p.z / res_z)); + + return autoware::pointcloud_divider::GridInfo<3>(x_id, y_id, z_id); +} + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__GRID_INFO_HPP_ diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_divider.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_divider.hpp new file mode 100644 index 00000000..ada0d29d --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_divider.hpp @@ -0,0 +1,178 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +// BSD 3-Clause License +// +// Copyright (c) 2023, MAP IV +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__PCD_DIVIDER_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__PCD_DIVIDER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#define PCL_NO_PRECOMPILE +#include "grid_info.hpp" +#include "pcd_io.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::pointcloud_divider +{ + +template +class PCDDivider +{ + typedef pcl::PointCloud PclCloudType; + typedef typename PclCloudType::Ptr PclCloudPtr; + typedef std::unordered_map, std::tuple> GridMapType; + typedef typename GridMapType::iterator GridMapItr; + typedef std::multimap GridMapSizeType; + typedef typename GridMapSizeType::iterator GridMapSizeItr; + +public: + explicit PCDDivider(const rclcpp::Logger & logger) : logger_(logger) {} + + // Functions to set input parameters + void setInput(const std::string & input_pcd_or_dir) { input_pcd_or_dir_ = input_pcd_or_dir; } + + void setOutputDir(const std::string & output_dir) + { + output_dir_ = output_dir; + tmp_dir_ = output_dir + "/tmp/"; + } + + void setPrefix(const std::string & prefix) { file_prefix_ = prefix; } + + void setConfig(const std::string & config) + { + config_file_ = config; + + paramInitialize(); + } + + // Used to manually set parameters when a config file is not available + void setGridSize(float res_x, float res_y) + { + grid_size_x_ = res_x; + grid_size_y_ = res_y; + g_grid_size_x_ = grid_size_x_ * 10; + g_grid_size_y_ = grid_size_y_ * 10; + } + + void setLargeGridMode(bool use_large_grid) { use_large_grid_ = use_large_grid; } + + void setLeafSize(float leaf_size) { leaf_size_ = leaf_size; } + + void setDebugMode(bool mode) { debug_mode_ = mode; } + + std::pair getGridSize() const + { + return std::pair(grid_size_x_, grid_size_y_); + } + + void run(); + void run(const std::vector & pcd_names); + +private: + std::string input_pcd_or_dir_, output_dir_, file_prefix_, config_file_; + + std::unordered_set> grid_set_; + + // Params from yaml + bool use_large_grid_ = false; + double leaf_size_ = 0.1; + double grid_size_x_ = 100; + double grid_size_y_ = 100; + double g_grid_size_x_ = grid_size_x_ * 10; + double g_grid_size_y_ = grid_size_y_ * 10; + + // Maximum number of points per PCD block + const size_t max_block_size_ = 500000; + + // Map of points distributed to grids + GridMapType grid_to_cloud_; + + // Segments but sorted by size + GridMapSizeType seg_by_size_; + // Map segments to size iterator + std::unordered_map, GridMapSizeItr> seg_to_size_itr_map_; + // Only 100 million points are allowed to reside in the main memory at max + const size_t max_resident_point_num_ = 100000000; + size_t resident_point_num_ = 0; + std::string tmp_dir_; + CustomPCDReader reader_; + bool debug_mode_ = true; // Print debug messages or not + rclcpp::Logger logger_; + + // Find all PCD files from the input path + std::vector discoverPCDs(const std::string & input); + + std::string makeFileName(const GridInfo<2> & grid) const; + + PclCloudPtr loadPCD(const std::string & pcd_name); + void savePCD(const std::string & pcd_name, const pcl::PointCloud & cloud); + void dividePointCloud(const PclCloudPtr & cloud_ptr); + void paramInitialize(); + void saveGridInfoToYAML(const std::string & yaml_file_path); + void checkOutputDirectoryValidity(); + + void saveGridPCD(GridMapItr & grid_it); + void saveTheRest(); + void mergeAndDownsample(); + void mergeAndDownsample( + const std::string & dir_path, std::list & pcd_list, size_t total_point_num); +}; + +} // namespace autoware::pointcloud_divider + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__PCD_DIVIDER_HPP_ diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io.hpp new file mode 100644 index 00000000..e55d98d5 --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io.hpp @@ -0,0 +1,21 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_HPP_ + +#include "pcd_io_reader.hpp" +#include "pcd_io_writer.hpp" + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_HPP_ diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_reader.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_reader.hpp new file mode 100644 index 00000000..76599070 --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_reader.hpp @@ -0,0 +1,603 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_READER_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_READER_HPP_ + +#include "utility.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace autoware::pointcloud_divider +{ + +#ifndef INVALID_LOC_ +#define INVALID_LOC_ (0xFFFF) +#endif + +template +class CustomPCDReader +{ + typedef pcl::PointCloud PclCloudType; + +public: + CustomPCDReader() + { + buffer_ = nullptr; + clear(); + } + + // Set a file to reading + void setInput(const std::string & pcd_path); + // Read a block of points from the input stream + size_t readABlock(PclCloudType & output); + + // Get path to the current opening PCD + const std::string & get_path() const { return pcd_path_; } + + void setBlockSize(size_t block_size) { block_size_ = block_size; } + + bool good() { return file_.good(); } + + size_t point_num() + { + if (file_.is_open()) { + return point_num_; + } + + return 0; + } + + ~CustomPCDReader() { clear(); } + +private: + void readHeader(std::ifstream & input); + size_t readABlock(std::ifstream & input, PclCloudType & output); + + size_t readABlockBinary(std::ifstream & input, PclCloudType & output); + size_t readABlockASCII(std::ifstream & input, PclCloudType & output); + + void clear() + { + version_.clear(); + width_ = height_ = 0; + field_names_.clear(); + field_sizes_.clear(); + field_types_.clear(); + field_counts_.clear(); + origin_x_ = origin_y_ = origin_z_ = 0.0; + orientation_w_ = orientation_x_ = orientation_y_ = orientation_z_ = 0.0; + point_num_ = 0; + binary_ = true; + + if (file_.is_open()) { + file_.close(); + } + + file_.clear(); + + point_size_ = read_size_ = 0; + if (buffer_) { + delete[] buffer_; + } + + buffer_ = nullptr; + block_size_ = 30000000; + } + + // Metadata + std::string version_; + size_t width_, height_; + std::vector field_names_; + std::vector field_sizes_; + std::vector field_types_; + std::vector field_counts_; + // Viewpoint + float origin_x_, origin_y_, origin_z_; + float orientation_w_, orientation_x_, orientation_y_, orientation_z_; + // Number of points in the PCD file + size_t point_num_; + bool binary_; // Data: true: binary, false: ascii + std::ifstream file_; // Input stream of the PCD file + size_t block_size_ = 30000000; // Number of points to read in each readABlock + size_t point_size_, read_size_; + char * buffer_; + std::string pcd_path_; // Path to the current opening PCD + std::vector read_loc_; + std::vector read_sizes_; +}; + +template +void CustomPCDReader::setInput(const std::string & pcd_path) +{ + clear(); + + file_.open(pcd_path); + + if (!file_.is_open()) { + fprintf( + stderr, "[%s, %d] %s::Error: Failed to open a file at %s\n", __FILE__, __LINE__, __func__, + pcd_path.c_str()); + exit(EXIT_FAILURE); + } + + pcd_path_ = pcd_path; + readHeader(file_); +} + +template +size_t CustomPCDReader::readABlock(PclCloudType & output) +{ + return readABlock(file_, output); +} + +// Build the location vectors for reading binary points +template +inline void buildReadMetadata( + std::vector & field_names, std::vector & field_sizes, + std::vector & field_counts, std::vector & read_loc, + std::vector & read_sizes); + +// Build the location vector for reading ascii points +template +inline void buildReadMetadataASCII( + std::vector & field_names, std::vector & read_loc); + +template +void CustomPCDReader::readHeader(std::ifstream & input) +{ + std::string line; + std::vector vals; // Each line is splitted into multiple values by a delimiter + + try { + while (input) { + std::getline(input, line); + + if (line.empty()) { + continue; + } + + util::split(line, " ", vals); + + // Read comments + if (vals[0] == "#") { + continue; + } + + if (vals[0] == "VERSION") { + version_ = vals[1]; + + continue; + } + + if (vals[0] == "FIELDS") { + for (size_t i = 1; i < vals.size(); ++i) { + field_names_.push_back(util::trim(vals[i])); + } + + continue; + } + + if (vals[0] == "SIZE") { + if (vals.size() - 1 != field_names_.size()) { + fprintf( + stderr, + "[%s, %d] %s::Error: Invalid PCD file format: Number of sizes is different from number " + "of fields. " + "File %s\n", + __FILE__, __LINE__, __func__, pcd_path_.c_str()); + exit(EXIT_FAILURE); + } + + for (size_t i = 1; i < vals.size(); ++i) { + field_sizes_.push_back(std::stoi(vals[i])); + } + + continue; + } + + if (vals[0] == "TYPE") { + if (vals.size() - 1 != field_names_.size()) { + fprintf( + stderr, + "[%s, %d] %s::Error: Invalid PCD file format: Number of types is different from number " + "of fields. " + "File %s\n", + __FILE__, __LINE__, __func__, pcd_path_.c_str()); + exit(EXIT_FAILURE); + } + + for (size_t i = 1; i < vals.size(); ++i) { + field_types_.push_back(util::trim(vals[i])); + } + + continue; + } + + if (vals[0] == "COUNT") { + if (vals.size() - 1 != field_names_.size()) { + fprintf( + stderr, + "[%s, %d] %s::Error: Invalid PCD file format: Number of counts is different from " + "number of fields. " + "File %s\n", + __FILE__, __LINE__, __func__, pcd_path_.c_str()); + exit(EXIT_FAILURE); + } + + for (size_t i = 1; i < vals.size(); ++i) { + field_counts_.push_back(std::stoi(vals[i])); + } + + continue; + } + + if (vals[0] == "WIDTH") { + width_ = std::stoi(vals[1]); + + continue; + } + + if (vals[0] == "HEIGHT") { + height_ = std::stoi(vals[1]); + + continue; + } + + if (vals[0] == "VIEWPOINT") { + if (vals.size() != 8) { + fprintf( + stderr, + "[%s, %d] %s::Error: Invalid PCD file format: Viewpoint must contain 7 fields. Number " + "of actual " + "fields: %lu. File %s\n", + __FILE__, __LINE__, __func__, vals.size() - 1, pcd_path_.c_str()); + exit(EXIT_FAILURE); + } + + origin_x_ = std::stof(vals[1]); + origin_y_ = std::stof(vals[2]); + origin_z_ = std::stof(vals[3]); + + orientation_w_ = std::stof(vals[4]); + orientation_x_ = std::stof(vals[5]); + orientation_y_ = std::stof(vals[6]); + orientation_z_ = std::stof(vals[7]); + + continue; + } + + if (vals[0] == "POINTS") { + point_num_ = std::stoi(vals[1]); + + continue; + } + + if (vals[0] == "DATA") { + binary_ = vals[1].find("binary") != std::string::npos; + + break; + } + + // After finish parsing fields, break + // TODO(AnhNguyen): Should I roll back to the previous position? + break; + } + } catch (...) { + fprintf( + stderr, "[%s, %d] %s::Error: Failed to read the PCD header from file %s\n", __FILE__, + __LINE__, __func__, pcd_path_.c_str()); + exit(EXIT_FAILURE); + } + + // Compute the number of bytes per reading + point_size_ = read_size_ = 0; + + for (size_t i = 0; i < field_sizes_.size(); ++i) { + point_size_ += field_sizes_[i] * field_counts_[i]; + } + + read_size_ = point_size_ * block_size_; + + if (buffer_) { + delete[] buffer_; + } + + buffer_ = new char[read_size_]; + + if (field_sizes_.size() > 0) { + // Construct read loc and read size, used to read data from files to points + if (binary_) { + buildReadMetadata(field_names_, field_sizes_, field_counts_, read_loc_, read_sizes_); + } else { + buildReadMetadataASCII(field_names_, read_loc_); + } + } +} + +inline void setFieldReadMetadata( + const std::string & field_tag, std::vector & field_names, + std::vector & field_sizes, std::vector & tmp_read_loc, size_t & read_loc, + size_t & read_size) +{ + for (size_t i = 0; i < field_names.size(); ++i) { + if (field_names[i] == field_tag) { + read_loc = tmp_read_loc[i]; + read_size = field_sizes[i]; + + return; + } + } + + // If the field tag was not found, set both read_loc and read_size to 0 + std::cerr << "Field " << field_tag << " is not available, and will be set to 0." << std::endl; + read_loc = 0; + read_size = 0; +} + +inline void setFieldReadMetadata( + const std::string & field_tag, std::vector & field_names, size_t & read_loc) +{ + for (size_t i = 0; i < field_names.size(); ++i) { + if (field_names[i] == field_tag) { + read_loc = i; + + return; + } + } + + // If the field tag was not found, set read_loc to -1 + std::cerr << "Field " << field_tag << " is not available, and will be set to 0." << std::endl; + read_loc = INVALID_LOC_; +} + +template <> +inline void buildReadMetadata( + std::vector & field_names, std::vector & field_sizes, + std::vector & field_counts, std::vector & read_loc, + std::vector & read_sizes) +{ + size_t field_num = field_names.size(); + + read_loc.resize(3); + read_sizes.resize(3); + + std::vector tmp_read_loc(field_num); + + tmp_read_loc[0] = 0; + + for (size_t i = 0; i < field_num - 1; ++i) { + tmp_read_loc[i + 1] = field_sizes[i] * field_counts[i] + tmp_read_loc[i]; + } + + // Find field x, y, z + setFieldReadMetadata("x", field_names, field_sizes, tmp_read_loc, read_loc[0], read_sizes[0]); + setFieldReadMetadata("y", field_names, field_sizes, tmp_read_loc, read_loc[1], read_sizes[1]); + setFieldReadMetadata("z", field_names, field_sizes, tmp_read_loc, read_loc[2], read_sizes[2]); +} + +template <> +inline void buildReadMetadata( + std::vector & field_names, std::vector & field_sizes, + std::vector & field_counts, std::vector & read_loc, + std::vector & read_sizes) +{ + size_t field_num = field_names.size(); + + read_loc.resize(4); + read_sizes.resize(4); + + std::vector tmp_read_loc(field_num); + + tmp_read_loc[0] = 0; + + for (size_t i = 0; i < field_num - 1; ++i) { + tmp_read_loc[i + 1] = field_sizes[i] * field_counts[i] + tmp_read_loc[i]; + } + + // Find field x, y, z, intensity + setFieldReadMetadata("x", field_names, field_sizes, tmp_read_loc, read_loc[0], read_sizes[0]); + setFieldReadMetadata("y", field_names, field_sizes, tmp_read_loc, read_loc[1], read_sizes[1]); + setFieldReadMetadata("z", field_names, field_sizes, tmp_read_loc, read_loc[2], read_sizes[2]); + setFieldReadMetadata( + "intensity", field_names, field_sizes, tmp_read_loc, read_loc[3], read_sizes[3]); +} + +template <> +inline void buildReadMetadataASCII( + std::vector & field_names, std::vector & read_loc) +{ + read_loc.resize(3); + + setFieldReadMetadata("x", field_names, read_loc[0]); + setFieldReadMetadata("y", field_names, read_loc[1]); + setFieldReadMetadata("z", field_names, read_loc[2]); +} + +template <> +inline void buildReadMetadataASCII( + std::vector & field_names, std::vector & read_loc) +{ + read_loc.resize(4); + + setFieldReadMetadata("x", field_names, read_loc[0]); + setFieldReadMetadata("y", field_names, read_loc[1]); + setFieldReadMetadata("z", field_names, read_loc[2]); + setFieldReadMetadata("intensity", field_names, read_loc[3]); +} + +template +inline void parsePoint( + const char * input, std::vector & rsize, std::vector & loc, PointT & output); + +template <> +inline void parsePoint( + const char * input, std::vector & rsize, std::vector & loc, + pcl::PointXYZ & output) +{ + memcpy(&(output.x), input + loc[0], rsize[0]); + memcpy(&(output.y), input + loc[1], rsize[1]); + memcpy(&(output.z), input + loc[2], rsize[2]); +} + +template <> +inline void parsePoint( + const char * input, std::vector & rsize, std::vector & loc, + pcl::PointXYZI & output) +{ + memcpy(&(output.x), input + loc[0], rsize[0]); + memcpy(&(output.y), input + loc[1], rsize[1]); + memcpy(&(output.z), input + loc[2], rsize[2]); + memcpy(&(output.intensity), input + loc[3], rsize[3]); +} + +template +size_t CustomPCDReader::readABlockBinary(std::ifstream & input, PclCloudType & output) +{ + output.clear(); + output.reserve(block_size_); + + PointT p; + + if (input) { + input.read(buffer_, read_size_); + + // Parse the buffer and convert to point + for (int i = 0; i < input.gcount(); i += point_size_) { + parsePoint(buffer_ + i, read_sizes_, read_loc_, p); + output.push_back(p); + } + + return input.gcount(); + } + + return 0; +} + +template +void parsePoint(const std::string & point_line, const std::vector & loc, PointT & output); + +template <> +inline void parsePoint( + const std::string & point_line, const std::vector & loc, pcl::PointXYZ & output) +{ + std::vector vals; + + util::split(point_line, " ", vals); + output.x = (loc[0] != INVALID_LOC_) ? std::stof(vals[loc[0]]) : 0; + output.y = (loc[1] != INVALID_LOC_) ? std::stof(vals[loc[1]]) : 0; + output.z = (loc[2] != INVALID_LOC_) ? std::stof(vals[loc[2]]) : 0; +} + +template <> +inline void parsePoint( + const std::string & point_line, const std::vector & loc, pcl::PointXYZI & output) +{ + std::vector vals; + + util::split(point_line, " ", vals); + output.x = (loc[0] != INVALID_LOC_) ? std::stof(vals[loc[0]]) : 0; + output.y = (loc[1] != INVALID_LOC_) ? std::stof(vals[loc[1]]) : 0; + output.z = (loc[2] != INVALID_LOC_) ? std::stof(vals[loc[2]]) : 0; + output.intensity = (loc[3] != INVALID_LOC_) ? std::stof(vals[loc[3]]) : 0; +} + +template +size_t CustomPCDReader::readABlockASCII(std::ifstream & input, PclCloudType & output) +{ + output.clear(); + output.reserve(block_size_); + + std::string point_line; + + PointT p; + + while (input) { + std::getline(input, point_line); + + if (!input.fail()) { + // Parse the buffer and convert to point + parsePoint(point_line, read_loc_, p); + output.push_back(p); + } else { + fprintf( + stderr, "[%s, %d] %s::Error: Failed to read a block of points from file. File %s\n", + __FILE__, __LINE__, __func__, pcd_path_.c_str()); + exit(EXIT_FAILURE); + } + + return input.gcount(); + } + + return 0; +} + +template +size_t CustomPCDReader::readABlock(std::ifstream & input, PclCloudType & output) +{ + if (binary_) { + return readABlockBinary(input, output); + } + + return readABlockASCII(input, output); +} + +} // namespace autoware::pointcloud_divider + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_READER_HPP_ diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_writer.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_writer.hpp new file mode 100644 index 00000000..487797c5 --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_writer.hpp @@ -0,0 +1,458 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_WRITER_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_WRITER_HPP_ + +#include "utility.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::pointcloud_divider +{ + +template +class CustomPCDWriter +{ + typedef pcl::PointCloud PclCloudType; + +public: + CustomPCDWriter() + { + buffer_ = nullptr; + point_num_ = 0; + written_point_num_ = 0; + binary_ = true; + point_size_ = 0; + write_size_ = 0; + + init(); + } + + // Set a file for writing + void setOutput(const std::string & pcd_path); + // Write metadata to the output + void writeMetadata(size_t point_num, bool binary_mode); + // Write a block of points to the output stream + void write(const PclCloudType & input); + + // Get path to the current opening PCD + const std::string & get_path() const { return pcd_path_; } + + void setBlockSize(size_t block_size) { block_size_ = block_size; } + + bool good() { return file_.good(); } + + ~CustomPCDWriter() { clear(); } + +private: + void writeABlockBinary(const PclCloudType & input, size_t loc, size_t proc_size); + void writeABlockASCII(const PclCloudType & input, size_t loc, size_t proc_size); + + // If the number of points written to file is less than the number of points + // in the header, fill the remaining points with 0. This often indicates + // that there are something wrong with the processing, and filling by 0 helps + // analyze the problem easier. + void padding(); + + void clear() + { + padding(); + + fields_.clear(); + field_sizes_.clear(); + point_num_ = 0; + written_point_num_ = 0; + binary_ = true; + + if (file_.is_open()) { + file_.close(); + } + + file_.clear(); + + point_size_ = write_size_ = 0; + if (buffer_) { + delete[] buffer_; + } + + buffer_ = nullptr; + block_size_ = 30000000; + } + + void init() + { + // Remember field sizes + fields_ = pcl::getFields(); + size_t i = 0; + + for (const auto & field : fields_) { + // Remove unidentified fields + if (field.name == "_") { + continue; + } + + size_t fs = field.count * pcl::getFieldSize(field.datatype); + + field_sizes_.push_back(fs); + fields_[i++] = field; + point_size_ += fs; + } + + fields_.resize(i); + + // Reserve a buffer for writing + write_size_ = point_size_ * block_size_; + buffer_ = new char[write_size_]; + } + + // Metadata + std::vector fields_; + std::vector field_sizes_; + + // Number of points in the PCD file + size_t point_num_; + bool binary_; // Data: true: binary, false: ascii + std::ofstream file_; // Input stream of the PCD file + size_t block_size_ = 30000000; // Maximum number of points to write in each writeABlock + size_t point_size_, write_size_; + char * buffer_; + std::string pcd_path_; // Path to the current opening PCD + size_t written_point_num_; // To track the number of points written to the file +}; + +template +void CustomPCDWriter::setOutput(const std::string & pcd_path) +{ + clear(); + init(); + + file_.open(pcd_path); + + if (!file_.is_open()) { + fprintf( + stderr, "[%s, %d] %s::Error: Failed to open a file at %s\n", __FILE__, __LINE__, __func__, + pcd_path.c_str()); + exit(EXIT_FAILURE); + } + + pcd_path_ = pcd_path; +} + +template +void CustomPCDWriter::writeMetadata(size_t point_num, bool binary_mode) +{ + if (!file_.is_open()) { + fprintf( + stderr, "[%s, %d] %s::Error: File is not opening at %s!\n", __FILE__, __LINE__, __func__, + pcd_path_.c_str()); + exit(EXIT_FAILURE); + } + + pcl::PCDWriter pcd_writer; + std::string pcd_metadata = pcd_writer.generateHeader(PclCloudType(), point_num); + + file_ << pcd_metadata; + + binary_ = binary_mode; + point_num_ = point_num; + + if (binary_) { + file_ << "DATA binary" << std::endl; + } else { + file_ << "DATA ascii" << std::endl; + } +} + +template +void CustomPCDWriter::write(const PclCloudType & input) +{ + for (size_t read_loc = 0; read_loc < input.size(); read_loc += block_size_) { + size_t proc_size = + (read_loc + block_size_ < input.size()) ? block_size_ : input.size() - read_loc; + + if (binary_) { + writeABlockBinary(input, read_loc, proc_size); + } else { + writeABlockASCII(input, read_loc, proc_size); + } + } + + written_point_num_ += input.size(); +} + +template +void CustomPCDWriter::writeABlockBinary( + const PclCloudType & input, size_t loc, size_t proc_size) +{ + // Read points to the write buffer + for (size_t i = loc, write_loc = 0; i < proc_size; ++i) { + const char * p = reinterpret_cast(&input[i]); + + for (size_t fid = 0; fid < fields_.size(); ++fid) { + memcpy(buffer_ + write_loc, p + fields_[fid].offset, field_sizes_[fid]); + write_loc += field_sizes_[fid]; + } + } + + // Write the buffer to the file + file_.write(buffer_, proc_size * point_size_); +} + +template +void CustomPCDWriter::writeABlockASCII( + const PclCloudType & cloud, size_t loc, size_t proc_size) +{ + // To reduce I/O cost, write several points together + std::ostringstream stream; + // When the number of points in the stream reach this number, write the stream to file + const size_t max_point_num = 10; + size_t packed_point_num = 0; + + for (size_t i = loc; i < proc_size; ++i) { + auto & point = cloud[i]; + + // Copied from the PCL library + for (size_t d = 0; d < fields_.size(); ++d) { + // Ignore invalid padded dimensions that are inherited from binary data + if (fields_[d].name == "_") continue; + + int count = fields_[d].count; + if (count == 0) + count = 1; // we simply cannot tolerate 0 counts (coming from older converter code) + + for (int c = 0; c < count; ++c) { + switch (fields_[d].datatype) { + // TODO(anh.nguyen.2@tier4.jp): PCL 1.12 has not supported BOOL, INT64, and UINT64 yet + // case pcl::PCLPointField::BOOL: + // { + // bool value; + // memcpy (&value, reinterpret_cast(&point) + fields_[d].offset + c * + // sizeof(bool), sizeof(bool)); stream << boost::numeric_cast(value); + // break; + // } + case pcl::PCLPointField::INT8: { + std::int8_t value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + c * sizeof(std::int8_t), + sizeof(std::int8_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT8: { + std::uint8_t value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + c * sizeof(std::uint8_t), + sizeof(std::uint8_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::INT16: { + std::int16_t value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + c * sizeof(std::int16_t), + sizeof(std::int16_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT16: { + std::uint16_t value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + + c * sizeof(std::uint16_t), + sizeof(std::uint16_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::INT32: { + std::int32_t value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + c * sizeof(std::int32_t), + sizeof(std::int32_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT32: { + std::uint32_t value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + + c * sizeof(std::uint32_t), + sizeof(std::uint32_t)); + stream << boost::numeric_cast(value); + break; + } + // case pcl::PCLPointField::INT64: + // { + // std::int64_t value; + // memcpy (&value, reinterpret_cast(&point) + fields_[d].offset + c * + // sizeof(std::int64_t), sizeof(std::int64_t)); stream << + // boost::numeric_cast(value); break; + // } + // case pcl::PCLPointField::UINT64: + // { + // std::uint64_t value; + // memcpy (&value, reinterpret_cast(&point) + fields_[d].offset + c * + // sizeof(std::uint64_t), sizeof(std::uint64_t)); stream << + // boost::numeric_cast(value); break; + // } + case pcl::PCLPointField::FLOAT32: { + /* + * Despite the float type, store the rgb field as uint32 + * because several fully opaque color values are mapped to + * nan. + */ + if ("rgb" == fields_[d].name) { + std::uint32_t value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + c * sizeof(float), + sizeof(float)); + stream << boost::numeric_cast(value); + break; + } + float value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + c * sizeof(float), + sizeof(float)); + if (std::isnan(value)) + stream << "nan"; + else + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::FLOAT64: { + double value; + memcpy( + &value, + reinterpret_cast(&point) + fields_[d].offset + c * sizeof(double), + sizeof(double)); + if (std::isnan(value)) + stream << "nan"; + else + stream << boost::numeric_cast(value); + break; + } + default: + fprintf( + stderr, "[%s, %d] %s::Incorrect field data type specified (%d)!\n", __FILE__, + __LINE__, __func__, fields_[d].datatype); + break; + } + + if (d < fields_.size() - 1 || c < static_cast(fields_[d].count - 1)) { + stream << " "; + } + } + } + + stream << "\n"; + + ++packed_point_num; + + if (packed_point_num == max_point_num) { + // Copy the stream, trim it, and write it to disk + std::string result = stream.str(); + boost::trim(result); + stream.str(""); + file_ << result << "\n"; + packed_point_num = 0; + } + } + + // Write the rest of stream to file + if (packed_point_num > 0) { + // Copy the stream, trim it, and write it to disk + std::string result = stream.str(); + boost::trim(result); + stream.str(""); + file_ << result << "\n"; + packed_point_num = 0; + } +} + +template +void CustomPCDWriter::padding() +{ + if (written_point_num_ < point_num_) { + PclCloudType all_zeros; + + all_zeros.resize(point_num_ - written_point_num_); + + PointT zp; + + memset(&zp, 0x00, sizeof(PointT)); + + std::fill(all_zeros.begin(), all_zeros.end(), zp); + + write(all_zeros); + } +} + +} // namespace autoware::pointcloud_divider + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__PCD_IO_WRITER_HPP_ diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/utility.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/utility.hpp new file mode 100644 index 00000000..544ae4f9 --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/utility.hpp @@ -0,0 +1,133 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__UTILITY_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__UTILITY_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_divider::util +{ +// Create a directory +inline bool make_dir(const std::string & path) +{ + std::string cmd = "mkdir -p " + path; + std::string err_msg = "Error: Failed to execute command " + cmd; + + if (std::system(cmd.c_str())) { + perror(err_msg.c_str()); + exit(EXIT_FAILURE); + } + + return true; +} + +// Remove a file/directory at @path +inline bool remove(const std::string & path) +{ + if (!std::filesystem::remove_all(path)) { + std::cerr << "Error: File at " << path << " does not exist." << std::endl; + return false; + } + + return true; +} + +// Parse the name of the PCD file, and return the number of points in the file +// Can only be used for file names of "***_.pcd" format +inline size_t point_num(const std::string & pcd_path) +{ + // Get the file name only + auto last_underbar_pos = pcd_path.rfind("_"); + auto last_dot_pos = pcd_path.rfind("."); + auto point_num_str = pcd_path.substr(last_underbar_pos + 1, last_dot_pos - last_underbar_pos - 1); + + return std::stoi(point_num_str); +} + +/** + * Split a line into multiple strings using the input + * delimiter. Output strings are stored into a vector + * of strings. + * + * Params: + * @line[in] : the input line to be splitted + * @del[in] : the delimiter + * @vals[out] : the output vector of strings + * + */ +inline int split(const std::string & line, const std::string & del, std::vector & vals) +{ + vals.clear(); + size_t start = 0, end = 0; + + do { + end = line.find(del, start); + + if (end != std::string::npos) { + vals.push_back(line.substr(start, end - start)); + + start = end + del.size(); + } + } while (end != std::string::npos); + + vals.push_back(line.substr(start)); + + return vals.size(); +} + +template +inline void zero_point(PointT & p); + +template <> +inline void zero_point(pcl::PointXYZ & p) +{ + p.x = p.y = p.z = 0; +} + +template <> +inline void zero_point(pcl::PointXYZI & p) +{ + p.x = p.y = p.z = p.intensity = 0; +} + +// Remove trailing whitespace, newline, and carriage return characters from a string +inline std::string trim(const std::string & input) +{ + std::string output; + + if (!input.empty()) { + output.reserve(input.size()); + + for (auto & c : input) { + if (c != ' ' && c != '\n' && c != '\r') { + output.push_back(c); + } + } + } + + return output; +} + +} // namespace autoware::pointcloud_divider::util + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__UTILITY_HPP_ diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/voxel_grid_filter.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/voxel_grid_filter.hpp new file mode 100644 index 00000000..f37d88fc --- /dev/null +++ b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/voxel_grid_filter.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef AUTOWARE__POINTCLOUD_DIVIDER__VOXEL_GRID_FILTER_HPP_ +#define AUTOWARE__POINTCLOUD_DIVIDER__VOXEL_GRID_FILTER_HPP_ + +#include +#include + +#include + +namespace autoware::pointcloud_divider +{ + +template +class VoxelGridFilter +{ + typedef pcl::PointCloud PclCloudType; + typedef typename PclCloudType::Ptr PclCloudPtr; + +public: + VoxelGridFilter() { resolution_ = 0; } + + void setResolution(float res) + { + if (res > 0) { + resolution_ = res; + } + } + + void filter(const PclCloudType & input, PclCloudType & output); + +private: + float resolution_; +}; + +template class VoxelGridFilter; +template class VoxelGridFilter; + +} // namespace autoware::pointcloud_divider + +#endif // AUTOWARE__POINTCLOUD_DIVIDER__VOXEL_GRID_FILTER_HPP_ diff --git a/map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.py b/map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.py new file mode 100644 index 00000000..f2fe228a --- /dev/null +++ b/map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.py @@ -0,0 +1,85 @@ +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# 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. + +import os + +from ament_index_python import get_package_share_path +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + arg = DeclareLaunchArgument(name, default_value=default_value, description=description) + launch_arguments.append(arg) + + add_launch_arg("input_pcd_dir", "", "Path to the folder containing the input PCD files") + add_launch_arg("output_pcd_dir", "", "Path to the folder containing the segmented PCD files") + add_launch_arg("prefix", "", "Prefix for the name of the output PCD files") + add_launch_arg( + "config_file", + [FindPackageShare("autoware_pointcloud_divider"), "/config/pointcloud_divider.param.yaml"], + "Path to the parameter file of the package autoware_pointcloud_divider", + ) + + # Parameter of pointcloud divider + config_file = os.path.join( + get_package_share_path("autoware_pointcloud_divider"), + "config", + "pointcloud_divider.param.yaml", + ) + with open(config_file, "r") as f: + divider_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + divider_node = Node( + package="autoware_pointcloud_divider", + executable="autoware_pointcloud_divider_node", + parameters=[ + divider_param, + { + "input_pcd_or_dir": LaunchConfiguration("input_pcd_or_dir"), + "output_pcd_dir": LaunchConfiguration("output_pcd_dir"), + "prefix": LaunchConfiguration("prefix"), + "config_file": LaunchConfiguration("config_file"), + }, + ], + output="screen", + ) + divider_shutdown = get_shutdown_handler(divider_node) + + return launch.LaunchDescription( + launch_arguments + + [ + divider_node, + divider_shutdown, + ] + ) + + +def get_shutdown_handler(node): + return launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=node, + on_exit=[ + launch.actions.LogInfo(msg="shutdown launch"), + launch.actions.EmitEvent(event=launch.events.Shutdown()), + ], + ) + ) diff --git a/map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.xml b/map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.xml new file mode 100644 index 00000000..f18054df --- /dev/null +++ b/map/autoware_pointcloud_divider/launch/pointcloud_divider.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/map/autoware_pointcloud_divider/package.xml b/map/autoware_pointcloud_divider/package.xml new file mode 100644 index 00000000..93acf847 --- /dev/null +++ b/map/autoware_pointcloud_divider/package.xml @@ -0,0 +1,25 @@ + + + + autoware_pointcloud_divider + 0.1.0 + A package for dividing PCD files to non-overlapped segments + Anh Nguyen + Apache License 2.0 + + Anh Nguyen + + ament_cmake_auto + autoware_cmake + + libpcl-all-dev + rclcpp + rclcpp_components + yaml-cpp + + ros2launch + + + ament_cmake + + diff --git a/map/autoware_pointcloud_divider/schema/pointcloud_divider.schema.json b/map/autoware_pointcloud_divider/schema/pointcloud_divider.schema.json new file mode 100644 index 00000000..d75b0555 --- /dev/null +++ b/map/autoware_pointcloud_divider/schema/pointcloud_divider.schema.json @@ -0,0 +1,68 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for autoware point cloud divider node", + "type": "object", + "definitions": { + "autoware_pointcloud_divider": { + "type": "object", + "properties": { + "use_large_grid": { + "type": "boolean", + "description": "Pack small segments to larger folders", + "default": "false" + }, + "leaf_size": { + "type": "number", + "description": "Resolution in meter for downsampling the output segments. Setting to negative to get the raw output PCDs.", + "default": "0.2" + }, + "grid_size_x": { + "type": "number", + "description": "The x size in meter of the output segments", + "default": "20" + }, + "grid_size_y": { + "type": "number", + "description": "The y size in meter of the output segments", + "default": "20" + }, + "input_pcd_or_dir": { + "type": "string", + "description": "The path to the folder containing the input PCD files", + "default": "" + }, + "output_pcd_dir": { + "type": "string", + "description": "The path to the folder containing the output PCD files", + "default": "" + }, + "prefix": { + "type": "string", + "description": "The prefix for the name of the output PCD files", + "default": "" + }, + "point_type": { + "type": "string", + "description": "Type of the point when processing PCD files. Could be point_xyz or point_xyzi", + "default": "point_xyzi" + } + }, + "required": ["grid_size_x", "grid_size_y", "input_pcd_or_dir", "output_pcd_dir", "prefix"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/autoware_pointcloud_divider" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/autoware_pointcloud_divider/src/include/pointcloud_divider_node.hpp b/map/autoware_pointcloud_divider/src/include/pointcloud_divider_node.hpp new file mode 100644 index 00000000..637e9a80 --- /dev/null +++ b/map/autoware_pointcloud_divider/src/include/pointcloud_divider_node.hpp @@ -0,0 +1,34 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef POINTCLOUD_DIVIDER_NODE_HPP_ +#define POINTCLOUD_DIVIDER_NODE_HPP_ + +#include + +#define PCL_NO_RECOMPILE +#include + +namespace autoware::pointcloud_divider +{ + +class PointCloudDivider : public rclcpp::Node +{ +public: + explicit PointCloudDivider(const rclcpp::NodeOptions & node_options); +}; + +} // namespace autoware::pointcloud_divider + +#endif // POINTCLOUD_DIVIDER_NODE_HPP_ diff --git a/map/autoware_pointcloud_divider/src/pcd_divider.cpp b/map/autoware_pointcloud_divider/src/pcd_divider.cpp new file mode 100644 index 00000000..b4468d37 --- /dev/null +++ b/map/autoware_pointcloud_divider/src/pcd_divider.cpp @@ -0,0 +1,477 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +// BSD 3-Clause License +// +// Copyright (c) 2023, MAP IV +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include + +#include +#include + +#include +#include + +namespace fs = std::filesystem; + +namespace autoware::pointcloud_divider +{ + +template +std::vector PCDDivider::discoverPCDs(const std::string & input) +{ + // Discover PCD files in the input_dir + std::vector pcd_list; + fs::path input_path(input); + + if (fs::is_directory(input_path)) { + RCLCPP_INFO(logger_, "Input PCD directory: %s", input.c_str()); + + for (auto & entry : fs::directory_iterator(input_path)) { + if (fs::is_regular_file(entry.symlink_status())) { + auto file_name = entry.path().string(); + auto extension = entry.path().extension().string(); + + if (extension == ".pcd" || extension == ".PCD") { + pcd_list.push_back(file_name); + } + } + } + } else if (fs::is_regular_file(input_path)) { + auto file_name = input_path.string(); + auto extension = input_path.extension().string(); + + if (extension == ".pcd" || extension == ".PCD") { + RCLCPP_INFO(logger_, "Input PCD file: %s", input.c_str()); + + pcd_list.push_back(file_name); + } else { + RCLCPP_ERROR(logger_, "Error: The input file is not PCD format %s", input.c_str()); + exit(EXIT_FAILURE); + } + } else { + RCLCPP_ERROR(logger_, "Error: Invalid input %s", input.c_str()); + exit(EXIT_FAILURE); + } + + return pcd_list; +} + +template +void PCDDivider::run() +{ + // Discover PCD files + auto pcd_list = discoverPCDs(input_pcd_or_dir_); + + // Process pcd files + run(pcd_list); +} + +template +void PCDDivider::run(const std::vector & pcd_names) +{ + checkOutputDirectoryValidity(); + + grid_set_.clear(); + + for (const std::string & pcd_name : pcd_names) { + if (!rclcpp::ok()) { + return; + } + + if (debug_mode_) { + RCLCPP_INFO(logger_, "Dividing file %s", pcd_name.c_str()); + } + + do { + auto cloud_ptr = loadPCD(pcd_name); + + dividePointCloud(cloud_ptr); + } while (reader_.good() && rclcpp::ok()); + } + + saveTheRest(); + + RCLCPP_INFO(logger_, "Merge and downsampling... "); + + // Now merge and downsample + mergeAndDownsample(); + + std::string yaml_file_path = output_dir_ + "/pointcloud_map_metadata.yaml"; + saveGridInfoToYAML(yaml_file_path); + + RCLCPP_INFO(logger_, "Done!"); +} + +template +void PCDDivider::checkOutputDirectoryValidity() +{ + tmp_dir_ = output_dir_ + "/tmp/"; + + if (fs::exists(tmp_dir_)) { + fs::remove_all(tmp_dir_); + } + + if (fs::exists(output_dir_)) { + fs::remove_all(output_dir_); + } + + util::make_dir(output_dir_ + "/pointcloud_map.pcd/"); + util::make_dir(tmp_dir_); +} + +template +typename pcl::PointCloud::Ptr PCDDivider::loadPCD(const std::string & pcd_name) +{ + if (pcd_name != reader_.get_path()) { + reader_.setInput(pcd_name); + } + + PclCloudPtr cloud_ptr(new PclCloudType); + + reader_.readABlock(*cloud_ptr); + + return cloud_ptr; +} + +template +void PCDDivider::savePCD(const std::string & path, const pcl::PointCloud & cloud) +{ + if (pcl::io::savePCDFileBinary(path, cloud) == -1) { + RCLCPP_ERROR(logger_, "Error: Cannot save PCD: %s", path.c_str()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } +} + +template +void PCDDivider::dividePointCloud(const PclCloudPtr & cloud_ptr) +{ + if (!cloud_ptr || cloud_ptr->size() <= 0) { + return; + } + + for (const PointT p : *cloud_ptr) { + if (!rclcpp::ok()) { + rclcpp::shutdown(); + exit(EXIT_SUCCESS); + } + + auto tmp = pointToGrid2(p, grid_size_x_, grid_size_y_); + auto it = grid_to_cloud_.find(tmp); + + // If the grid has not existed yet, create a new one + if (it == grid_to_cloud_.end()) { + auto & new_grid = grid_to_cloud_[tmp]; + + std::get<0>(new_grid).reserve(max_block_size_); + + std::get<0>(new_grid).push_back(p); // Push the first point to the cloud + std::get<1>(new_grid) = 0; // Counter set to 0 + std::get<2>(new_grid) = 0; // Prev size is 0 + } else { + auto & cloud = std::get<0>(it->second); + auto & prev_size = std::get<2>(it->second); + + cloud.push_back(p); + + ++resident_point_num_; + + // If the number of points in the segment reach maximum, save the segment to file + if (cloud.size() == max_block_size_) { + saveGridPCD(it); + } else { + // Otherwise, update the seg_by_size_ if the change of size is significant + if (cloud.size() - prev_size >= 10000) { + prev_size = cloud.size(); + auto seg_to_size_it = seg_to_size_itr_map_.find(tmp); + + if (seg_to_size_it == seg_to_size_itr_map_.end()) { + auto size_it = seg_by_size_.insert(std::make_pair(prev_size, it)); + seg_to_size_itr_map_[tmp] = size_it; + } else { + seg_by_size_.erase(seg_to_size_it->second); + auto new_size_it = seg_by_size_.insert(std::make_pair(prev_size, it)); + seg_to_size_it->second = new_size_it; + } + } + } + + // If the number of resident points reach maximum, save the biggest resident segment to SSD + if (resident_point_num_ >= max_resident_point_num_) { + auto max_size_seg_it = seg_by_size_.rbegin(); + + saveGridPCD(max_size_seg_it->second); + } + } + } +} + +template +void PCDDivider::saveGridPCD(GridMapItr & grid_it) +{ + auto & cloud = std::get<0>(grid_it->second); + auto & counter = std::get<1>(grid_it->second); + auto & prev_size = std::get<2>(grid_it->second); + + // Try to create a new directory to contain the cloud + std::ostringstream seg_path, file_path; + + seg_path << tmp_dir_ << "/" << grid_it->first << "/"; + file_path << seg_path.str() << counter << "_" << cloud.size() << ".pcd"; + + util::make_dir(seg_path.str()); + + if (pcl::io::savePCDFileBinary(file_path.str(), cloud)) { + RCLCPP_ERROR(logger_, "Error: Cannot save a PCD file at %s", file_path.str().c_str()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } + + resident_point_num_ -= cloud.size(); + + // Clear the content of the segment cloud and reserve space for further points + cloud.clear(); + cloud.reserve(max_block_size_); + ++counter; // Increase the counter so the next segment save will not overwrite the previously + // saved one + prev_size = 0; + + // Update the seg_by_size_ and seg_to_size_itr_map_ + auto it = seg_to_size_itr_map_.find(grid_it->first); + + if (it != seg_to_size_itr_map_.end()) { + seg_by_size_.erase(it->second); + seg_to_size_itr_map_.erase(it); + } +} + +template +void PCDDivider::saveTheRest() +{ + for (auto it = grid_to_cloud_.begin(); it != grid_to_cloud_.end(); ++it) { + auto & cloud = std::get<0>(it->second); + + if (cloud.size() > 0) { + saveGridPCD(it); + } + } +} + +template +void PCDDivider::mergeAndDownsample() +{ + // Scan the tmp directory and find the segment folders + fs::path tmp_path(tmp_dir_); + + // Count the number of sub folders, to estimate progress + int seg_num = 0; + + for (auto & entry : fs::directory_iterator(tmp_path)) { + if (fs::is_directory(entry.symlink_status())) { + ++seg_num; + } + } + + for (auto & tmp_dir_entry : fs::directory_iterator(tmp_path)) { + if (fs::is_directory(tmp_dir_entry.symlink_status())) { + if (debug_mode_) { + RCLCPP_INFO(logger_, "Saving segment %s", tmp_dir_entry.path().string().c_str()); + } + std::list pcd_list; + size_t total_point_num = 0; + + for (auto & seg_entry : fs::directory_iterator(tmp_dir_entry.path())) { + if (fs::is_regular_file(seg_entry.symlink_status())) { + auto fname = seg_entry.path().string(); + auto ext = fname.substr(fname.size() - 4); + + if (ext == ".pcd") { + pcd_list.push_back(fname); + total_point_num += util::point_num(fname); + } + } + } + + // Fuse all PCDs and downsample if necessary + mergeAndDownsample(tmp_dir_entry.path().string(), pcd_list, total_point_num); + } + } + + // Remove tmp dir + util::remove(tmp_dir_); +} + +template +void PCDDivider::mergeAndDownsample( + const std::string & dir_path, std::list & pcd_list, size_t total_point_num) +{ + PclCloudPtr new_cloud(new PclCloudType); + + new_cloud->reserve(total_point_num); + + // Merge all PCDs that belong to the specified segment to a single segment point cloud + for (auto & fname : pcd_list) { + PclCloudType seg_cloud; + + if (pcl::io::loadPCDFile(fname, seg_cloud)) { + RCLCPP_ERROR(logger_, "Error: Failed to open a PCD file at %s", fname.c_str()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } + + for (auto & p : seg_cloud) { + new_cloud->push_back(p); + } + } + + // Downsample if needed + if (leaf_size_ > 0) { + VoxelGridFilter vgf; + PclCloudPtr filtered_cloud(new PclCloudType); + + vgf.setResolution(leaf_size_); + vgf.filter(*new_cloud, *filtered_cloud); + + new_cloud = filtered_cloud; + } + + // Save the new_cloud + // Extract segment name only (format gx_gy) + int end_name, start_name; + + for (end_name = dir_path.size() - 1; end_name >= 0 && dir_path[end_name] == '/'; --end_name) { + } + for (start_name = end_name; start_name >= 0 && dir_path[start_name] != '/'; --start_name) { + } + + std::string seg_name_only = dir_path.substr(start_name + 1, end_name - start_name); + + // Parse the @seg_path to get the indices of the segment + auto underbar_pos = seg_name_only.rfind("_"); + int gx = std::stoi(seg_name_only.substr(0, underbar_pos)); + int gy = std::stoi(seg_name_only.substr(underbar_pos + 1)); + + grid_set_.insert(GridInfo<2>(gx, gy)); + + // Construct the path to save the new_cloud + std::string save_path; + + // If save large pcd was turned on, create a folder to contain segment pcds + if (use_large_grid_) { + int large_gx = static_cast(std::floor(static_cast(gx) / g_grid_size_x_)); + int large_gy = static_cast(std::floor(static_cast(gy) / g_grid_size_y_)); + std::string large_folder = output_dir_ + "/pointcloud_map.pcd/" + std::to_string(large_gx) + + "_" + std::to_string(large_gy) + "/"; + + // Create a new folder for the large grid + util::make_dir(large_folder); + + save_path = large_folder + file_prefix_ + "_" + seg_name_only + ".pcd"; + } else { + save_path = output_dir_ + "/pointcloud_map.pcd/" + file_prefix_ + "_" + seg_name_only + ".pcd"; + } + + // Save the merged (filtered) cloud + if (pcl::io::savePCDFileBinary(save_path, *new_cloud)) { + RCLCPP_ERROR(logger_, "Error: Failed to save a point cloud at %s", save_path.c_str()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } + + // Delete the folder containing the segments + util::remove(dir_path); +} + +template +std::string PCDDivider::makeFileName(const GridInfo<2> & grid) const +{ + return file_prefix_ + "_" + std::to_string(grid.ix) + "_" + std::to_string(grid.iy) + ".pcd"; +} + +template +void PCDDivider::paramInitialize() +{ + try { + YAML::Node conf = YAML::LoadFile(config_file_); + auto params = conf["/**"]["ros__parameters"]; + use_large_grid_ = params["use_large_grid"].as(); + leaf_size_ = params["leaf_size"].as(); + grid_size_x_ = params["grid_size_x"].as(); + grid_size_y_ = params["grid_size_y"].as(); + } catch (YAML::Exception & e) { + RCLCPP_ERROR(logger_, "YAML Error: %s", e.what()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } + + g_grid_size_x_ = grid_size_x_ * 10; + g_grid_size_y_ = grid_size_y_ * 10; +} + +template +void PCDDivider::saveGridInfoToYAML(const std::string & yaml_file_path) +{ + std::ofstream yaml_file(yaml_file_path); + + if (!yaml_file.is_open()) { + RCLCPP_ERROR(logger_, "Error: Cannot open YAML file: %s", yaml_file_path.c_str()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } + + yaml_file << "x_resolution: " << grid_size_x_ << std::endl; + yaml_file << "y_resolution: " << grid_size_y_ << std::endl; + + for (const auto & grid : grid_set_) { + std::string file_name = makeFileName(grid); + fs::path p(file_name); + yaml_file << p.filename().string() << ": [" << grid.ix << ", " << grid.iy << "]" << std::endl; + } + + yaml_file.close(); +} + +template class PCDDivider; +template class PCDDivider; + +} // namespace autoware::pointcloud_divider diff --git a/map/autoware_pointcloud_divider/src/pointcloud_divider_node.cpp b/map/autoware_pointcloud_divider/src/pointcloud_divider_node.cpp new file mode 100644 index 00000000..5ed7b0db --- /dev/null +++ b/map/autoware_pointcloud_divider/src/pointcloud_divider_node.cpp @@ -0,0 +1,95 @@ +// Copyright 2024 Autoware Foundation +// +// 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 "include/pointcloud_divider_node.hpp" + +#include + +#include + +namespace autoware::pointcloud_divider +{ + +PointCloudDivider::PointCloudDivider(const rclcpp::NodeOptions & node_options) +: Node("pointcloud_divider", node_options) +{ + // Load command parameters + bool use_large_grid = declare_parameter("use_large_grid", false); + float leaf_size = declare_parameter("leaf_size"); + float grid_size_x = declare_parameter("grid_size_x"); + float grid_size_y = declare_parameter("grid_size_y"); + std::string input_pcd_or_dir = declare_parameter("input_pcd_or_dir"); + std::string output_pcd_dir = declare_parameter("output_pcd_dir"); + std::string file_prefix = declare_parameter("prefix"); + std::string point_type = declare_parameter("point_type"); + // Enter a new line and clear it + // This is to get rid of the prefix of RCLCPP_INFO + std::string line_breaker(102, ' '); + + line_breaker[0] = '\n'; + line_breaker[1] = line_breaker[101] = '\r'; + + std::ostringstream param_display; + + param_display << line_breaker << line_breaker << "########## Input Parameters ##########" + << line_breaker; + + if (use_large_grid) { + param_display << "\tuse_large_grid: True" << line_breaker; + } else { + param_display << "\tuse_large_grid: False" << line_breaker; + } + + param_display << "\tleaf_size: " << leaf_size << line_breaker; + param_display << "\tgrid_size: " << grid_size_x << ", " << grid_size_y << line_breaker; + param_display << "\tinput_pcd_or_dir: " << input_pcd_or_dir << line_breaker; + param_display << "\toutput_pcd_dir: " << output_pcd_dir << line_breaker; + param_display << "\tfile_prefix: " << file_prefix << line_breaker; + param_display << "\tpoint_type: " << point_type << line_breaker; + param_display << "######################################" << line_breaker; + + RCLCPP_INFO(get_logger(), "%s", param_display.str().c_str()); + + if (point_type == "point_xyz") { + autoware::pointcloud_divider::PCDDivider pcd_divider_exe(get_logger()); + + pcd_divider_exe.setLargeGridMode(use_large_grid); + pcd_divider_exe.setLeafSize(leaf_size); + pcd_divider_exe.setGridSize(grid_size_x, grid_size_y); + pcd_divider_exe.setInput(input_pcd_or_dir); + pcd_divider_exe.setOutputDir(output_pcd_dir); + pcd_divider_exe.setPrefix(file_prefix); + + pcd_divider_exe.run(); + } else if (point_type == "point_xyzi") { + autoware::pointcloud_divider::PCDDivider pcd_divider_exe(get_logger()); + + pcd_divider_exe.setLargeGridMode(use_large_grid); + pcd_divider_exe.setLeafSize(leaf_size); + pcd_divider_exe.setGridSize(grid_size_x, grid_size_y); + pcd_divider_exe.setInput(input_pcd_or_dir); + pcd_divider_exe.setOutputDir(output_pcd_dir); + pcd_divider_exe.setPrefix(file_prefix); + + pcd_divider_exe.run(); + } + + rclcpp::shutdown(); +} + +} // namespace autoware::pointcloud_divider + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_divider::PointCloudDivider) diff --git a/map/autoware_pointcloud_divider/src/voxel_grid_filter.cpp b/map/autoware_pointcloud_divider/src/voxel_grid_filter.cpp new file mode 100644 index 00000000..7f06cdc5 --- /dev/null +++ b/map/autoware_pointcloud_divider/src/voxel_grid_filter.cpp @@ -0,0 +1,47 @@ +// Copyright 2024 Autoware Foundation +// +// 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 +#include +#include + +#include + +namespace autoware::pointcloud_divider +{ + +template +void VoxelGridFilter::filter(const PclCloudType & input, PclCloudType & output) +{ + if (resolution_ <= 0) { + return; + } + + std::unordered_map, Centroid> grid_map; + + for (auto & p : input) { + auto gkey = pointToGrid3(p, resolution_, resolution_, resolution_); + + grid_map[gkey].add(p); + } + + // Extract centroids + output.reserve(grid_map.size()); + + for (auto & it : grid_map) { + output.push_back(it.second.get()); + } +} + +} // namespace autoware::pointcloud_divider diff --git a/map/autoware_pointcloud_merger/CMakeLists.txt b/map/autoware_pointcloud_merger/CMakeLists.txt new file mode 100644 index 00000000..4183518b --- /dev/null +++ b/map/autoware_pointcloud_merger/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_pointcloud_merger) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Enable support for C++17 +if (${CMAKE_VERSION} VERSION_LESS "3.1.0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") +else () + set(CMAKE_CXX_STANDARD 17) +endif () + +# Find packages +find_package(yaml-cpp REQUIRED) +find_package(PCL REQUIRED) + +include_directories(include) + +ament_auto_add_library(${PROJECT_NAME} SHARED src/pointcloud_merger_node.cpp src/pcd_merger.cpp) +target_link_libraries(${PROJECT_NAME} yaml-cpp ${PCL_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} autoware_pointcloud_divider) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::pointcloud_merger::PointCloudMerger" + EXECUTABLE ${PROJECT_NAME}_node +) + +install(TARGETS ${PROJECT_NAME}_node + EXPORT ${PROJECT_NAME}_node + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include + ) + +ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/map/autoware_pointcloud_merger/LICENSE b/map/autoware_pointcloud_merger/LICENSE new file mode 100644 index 00000000..da934de9 --- /dev/null +++ b/map/autoware_pointcloud_merger/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2023, MAP IV + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/map/autoware_pointcloud_merger/README.md b/map/autoware_pointcloud_merger/README.md new file mode 100644 index 00000000..2197dda9 --- /dev/null +++ b/map/autoware_pointcloud_merger/README.md @@ -0,0 +1,48 @@ +# autoware_pointcloud_merger + +This is a tool for processing pcd files, and it can perform the following functions: + +- Merging multiple PCD files to a single PCD file +- Downsampling point clouds + +## Supported Data Format + +**Currently, only `pcl::PointXYZ` and `pcl::PointXYZI` are supported. Any PCD will be loaded as those two types .** + +This tool can be used with files that have data fields other than `XYZI` (e.g., `XYZRGB`) and files that only contain `XYZ`. + +- Data fields other than `XYZI` are ignored during loading. +- When loading `XYZ`-only data, the `intensity` field is assigned 0. + +## Installation + +```bash +cd # OR +cd src/ +git clone git@github.com:autowarefoundation/autoware_tools.git +cd .. +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests --symlink-install --packages-up-to autoware_pointcloud_merger +``` + +## Usage + +- Merger all PCD files from the input directory into a single output PCD + + ```bash + ros2 launch autoware_pointcloud_merger pointcloud_merger.launch.xml input_pcd_dir:= output_pcd:= + ``` + + | Name | Description | + | ---------- | ------------------------------------------- | + | INPUT_DIR | Directory that contains all input PCD files | + | OUTPUT_PCD | Name of the output PCD file | + +`INPUT_DIR` and `OUTPUT_PCD` should be specified as **absolute paths**. + +## Parameter + +{{ json_to_markdown("map/autoware_pointcloud_merger/schema/pointcloud_merger.schema.json") }} + +## LICENSE + +Parts of files pcd_merger.hpp, and pcd_merger.cpp are copied from [MapIV's pointcloud_divider](https://github.com/MapIV/pointcloud_divider) and are under [BSD-3-Clauses](LICENSE) license. The remaining code are under [Apache License 2.0](../../LICENSE) diff --git a/map/autoware_pointcloud_merger/config/pointcloud_merger.param.yaml b/map/autoware_pointcloud_merger/config/pointcloud_merger.param.yaml new file mode 100644 index 00000000..e91d4690 --- /dev/null +++ b/map/autoware_pointcloud_merger/config/pointcloud_merger.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + # Downsampling resolution + leaf_size: -0.1 + input_pcd_dir: $(var input_pcd_dir) # Path to the folder containing the input PCD Files + output_pcd: $(var output_pcd) # Path to the merged PCD File + point_type: "point_xyzi" # Type of points when processing PCD files diff --git a/map/autoware_pointcloud_merger/include/autoware/pointcloud_merger/pcd_merger.hpp b/map/autoware_pointcloud_merger/include/autoware/pointcloud_merger/pcd_merger.hpp new file mode 100644 index 00000000..a26abb37 --- /dev/null +++ b/map/autoware_pointcloud_merger/include/autoware/pointcloud_merger/pcd_merger.hpp @@ -0,0 +1,108 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +// BSD 3-Clause License +// +// Copyright (c) 2023, MAP IV +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ +#define AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ + +#include + +#include +#include + +#define PCL_NO_PRECOMPILE +#include +#include + +#include +#include + +namespace autoware::pointcloud_merger +{ + +template +class PCDMerger +{ + typedef pcl::PointCloud PclCloudType; + typedef typename PclCloudType::Ptr PclCloudPtr; + +public: + explicit PCDMerger(const rclcpp::Logger & logger) : logger_(logger) {} + + void setInput(const std::string & input) { input_dir_ = input; } + + void setOutput(const std::string & output) { output_pcd_ = output; } + + void setConfig(const std::string & config_file) + { + config_file_ = config_file; + + paramInitialize(); + } + + void setLeafSize(double leaf_size) { leaf_size_ = leaf_size; } + + void run(); + void run(const std::vector & pcd_names); + +private: + std::string output_pcd_, config_file_, input_dir_; + + // Params from yaml + double leaf_size_ = 0.1; + + // Maximum number of points per PCD block + const size_t max_block_size_ = 500000; + + std::string tmp_dir_; + autoware::pointcloud_divider::CustomPCDWriter writer_; + rclcpp::Logger logger_; + + std::vector discoverPCDs(const std::string & input); + void paramInitialize(); + void mergeWithoutDownsample(const std::vector & input_pcds); + void mergeWithDownsample(const std::vector & input_pcds); +}; + +} // namespace autoware::pointcloud_merger + +#endif // AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ diff --git a/map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.py b/map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.py new file mode 100644 index 00000000..f3a1607b --- /dev/null +++ b/map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.py @@ -0,0 +1,81 @@ +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# 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. + +import os + +from ament_index_python import get_package_share_path +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + arg = DeclareLaunchArgument(name, default_value=default_value, description=description) + launch_arguments.append(arg) + + add_launch_arg("input_pcd_dir", "", "Path to the folder containing the input PCD files") + add_launch_arg("output_pcd", "", "Path to the merged PCD file") + add_launch_arg( + "config_file", + [FindPackageShare("autoware_pointcloud_merger"), "/config/default.param.yaml"], + "Path to the parameter file of the package autoware_pointcloud_merger", + ) + + # Parameter of pointcloud divider + config_file = os.path.join( + get_package_share_path("autoware_pointcloud_merger"), "config", "default.param.yaml" + ) + with open(config_file, "r") as f: + merger_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + merger_node = Node( + package="autoware_pointcloud_merger", + executable="autoware_pointcloud_merger_node", + parameters=[ + merger_param, + { + "input_pcd_dir": LaunchConfiguration("input_pcd_dir"), + "output_pcd": LaunchConfiguration("output_pcd"), + "config_file": LaunchConfiguration("config_file"), + }, + ], + output="screen", + ) + merger_shutdown = get_shutdown_handler(merger_node) + + return launch.LaunchDescription( + launch_arguments + + [ + merger_node, + merger_shutdown, + ] + ) + + +def get_shutdown_handler(node): + return launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=node, + on_exit=[ + launch.actions.LogInfo(msg="shutdown launch"), + launch.actions.EmitEvent(event=launch.events.Shutdown()), + ], + ) + ) diff --git a/map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.xml b/map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.xml new file mode 100644 index 00000000..082ffb2c --- /dev/null +++ b/map/autoware_pointcloud_merger/launch/pointcloud_merger.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/map/autoware_pointcloud_merger/package.xml b/map/autoware_pointcloud_merger/package.xml new file mode 100644 index 00000000..08ac368b --- /dev/null +++ b/map/autoware_pointcloud_merger/package.xml @@ -0,0 +1,22 @@ + + + + autoware_pointcloud_merger + 0.1.0 + A package for dividing PCD files to non-overlapped segments + Anh Nguyen + Apache License 2.0 + + Anh Nguyen + + ament_cmake_auto + autoware_cmake + + autoware_pointcloud_divider + libpcl-all-dev + yaml-cpp + + + ament_cmake + + diff --git a/map/autoware_pointcloud_merger/schema/pointcloud_merger.schema.json b/map/autoware_pointcloud_merger/schema/pointcloud_merger.schema.json new file mode 100644 index 00000000..21311d69 --- /dev/null +++ b/map/autoware_pointcloud_merger/schema/pointcloud_merger.schema.json @@ -0,0 +1,48 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for autoware point cloud merger node", + "type": "object", + "definitions": { + "autoware_pointcloud_merger": { + "type": "object", + "properties": { + "leaf_size": { + "type": "number", + "description": "Resolution in meter for downsampling the output PCD. Setting to negative to get the raw output PCD.", + "default": "-0.1" + }, + "input_pcd_dir": { + "type": "string", + "description": "The path to the folder containing the input PCD files", + "default": "" + }, + "output_pcd": { + "type": "string", + "description": "The path to the merged PCD file", + "default": "" + }, + "point_type": { + "type": "string", + "description": "Type of the point when processing PCD files. Could be point_xyz or point_xyzi", + "default": "point_xyzi" + } + }, + "required": ["input_pcd_dir", "output_pcd"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/autoware_pointcloud_merger" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/autoware_pointcloud_merger/src/include/pointcloud_merger_node.hpp b/map/autoware_pointcloud_merger/src/include/pointcloud_merger_node.hpp new file mode 100644 index 00000000..2cecbb45 --- /dev/null +++ b/map/autoware_pointcloud_merger/src/include/pointcloud_merger_node.hpp @@ -0,0 +1,34 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef POINTCLOUD_MERGER_NODE_HPP_ +#define POINTCLOUD_MERGER_NODE_HPP_ + +#include + +#define PCL_NO_RECOMPILE +#include + +namespace autoware::pointcloud_merger +{ + +class PointCloudMerger : public rclcpp::Node +{ +public: + explicit PointCloudMerger(const rclcpp::NodeOptions & node_options); +}; + +} // namespace autoware::pointcloud_merger + +#endif // POINTCLOUD_MERGER_NODE_HPP_ diff --git a/map/autoware_pointcloud_merger/src/pcd_merger.cpp b/map/autoware_pointcloud_merger/src/pcd_merger.cpp new file mode 100644 index 00000000..65883dfe --- /dev/null +++ b/map/autoware_pointcloud_merger/src/pcd_merger.cpp @@ -0,0 +1,219 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +// BSD 3-Clause License +// +// Copyright (c) 2023, MAP IV +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "include/pointcloud_merger_node.hpp" + +#include +#include +#include + +#include + +#include + +namespace fs = std::filesystem; + +namespace autoware::pointcloud_merger +{ + +template +std::vector PCDMerger::discoverPCDs(const std::string & input) +{ + fs::path input_path(input); + + if (!fs::is_directory(input_path)) { + RCLCPP_ERROR(logger_, "Error: Invalid input directory %s", input.c_str()); + rclcpp::shutdown(); + } + + std::vector pcd_list; + + for (auto & entry : fs::directory_iterator(input_path)) { + if (fs::is_regular_file(entry.symlink_status())) { + auto file_name = entry.path().string(); + auto extension = entry.path().extension().string(); + + if (extension == ".pcd" || extension == ".PCD") { + pcd_list.push_back(file_name); + } + } + } + + RCLCPP_INFO(logger_, "Found %lu PCD files", pcd_list.size()); + + return pcd_list; +} + +template +void PCDMerger::run() +{ + auto pcd_list = discoverPCDs(input_dir_); + + run(pcd_list); +} + +template +void PCDMerger::run(const std::vector & pcd_names) +{ + // Just in case the downsampling option is on + if (leaf_size_ > 0) { + tmp_dir_ = "./pointcloud_merger_tmp/"; + + if (fs::exists(tmp_dir_)) { + fs::remove_all(tmp_dir_); + } + + autoware::pointcloud_divider::util::make_dir(tmp_dir_); + } + + if (fs::exists(output_pcd_)) { + fs::remove_all(output_pcd_); + } + + if (leaf_size_ > 0) { + mergeWithDownsample(pcd_names); + autoware::pointcloud_divider::util::remove(tmp_dir_); + } else { + mergeWithoutDownsample(pcd_names); + } +} + +template +void PCDMerger::mergeWithDownsample(const std::vector & input_pcds) +{ + RCLCPP_INFO(logger_, "Downsampling by PointCloudDivider"); + // Divide the input clouds to smaller segments + autoware::pointcloud_divider::PCDDivider pcd_divider(logger_); + + pcd_divider.setOutputDir(tmp_dir_); + pcd_divider.setGridSize(leaf_size_ * 100, leaf_size_ * 100); + pcd_divider.setPrefix("tmp_segment"); + pcd_divider.setLeafSize(leaf_size_); + pcd_divider.setDebugMode(false); + pcd_divider.run(input_pcds); + + // Now go get path of the segmentation pcd + std::vector seg_names; + fs::path tmp_path(tmp_dir_); + + for (auto & entry : fs::directory_iterator(tmp_path)) { + if (fs::is_regular_file(entry.symlink_status())) { + auto fname = entry.path().string(); + auto ext = fname.substr(fname.size() - 4); + + if (ext == ".pcd") { + seg_names.push_back(fname); + } + } + } + + // Now merge the downsampled segments + mergeWithoutDownsample(seg_names); +} + +template +void PCDMerger::mergeWithoutDownsample(const std::vector & input_pcds) +{ + if (input_pcds.size() == 0) { + RCLCPP_INFO(logger_, "No input PCDs. Return!"); + + return; + } + + // Check the number of points of the merger + size_t total_point_num = 0; + autoware::pointcloud_divider::CustomPCDReader reader; + + for (const auto & pcd_name : input_pcds) { + reader.setInput(pcd_name); + + total_point_num += reader.point_num(); + } + + writer_.setOutput(output_pcd_); + writer_.writeMetadata(total_point_num, true); + + size_t file_counter = 0; + + for (const auto & pcd_name : input_pcds) { + if (!rclcpp::ok()) { + return; + } + + RCLCPP_INFO( + logger_, "Processing file [%lu/%lu] %s", file_counter, input_pcds.size(), pcd_name.c_str()); + ++file_counter; + + reader.setInput(pcd_name); + + do { + PclCloudType new_cloud; + + reader.readABlock(new_cloud); + writer_.write(new_cloud); + } while (reader.good() && rclcpp::ok()); + } +} + +template +void PCDMerger::paramInitialize() +{ + try { + YAML::Node conf = YAML::LoadFile(config_file_); + auto params = conf["/**"]["ros__parameters"]; + + leaf_size_ = params["leaf_size"].as(); + } catch (YAML::Exception & e) { + RCLCPP_ERROR(logger_, "YAML Error: %s", e.what()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } +} + +template class PCDMerger; +template class PCDMerger; +// template class PCDMerger; +// template class PCDMerger; +// template class PCDMerger; + +} // namespace autoware::pointcloud_merger diff --git a/map/autoware_pointcloud_merger/src/pointcloud_merger_node.cpp b/map/autoware_pointcloud_merger/src/pointcloud_merger_node.cpp new file mode 100644 index 00000000..fe849891 --- /dev/null +++ b/map/autoware_pointcloud_merger/src/pointcloud_merger_node.cpp @@ -0,0 +1,78 @@ +// Copyright 2024 Autoware Foundation +// +// 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 "include/pointcloud_merger_node.hpp" + +#include + +#include + +namespace autoware::pointcloud_merger +{ + +PointCloudMerger::PointCloudMerger(const rclcpp::NodeOptions & node_options) +: Node("pointcloud_merger", node_options) +{ + // Load command parameters + float leaf_size = declare_parameter("leaf_size"); + std::string input_pcd_dir = declare_parameter("input_pcd_dir"); + std::string output_pcd = declare_parameter("output_pcd"); + std::string point_type = declare_parameter("point_type"); + + // Enter a new line and clear it + // This is to get rid of the prefix of RCLCPP_INFO + std::string line_breaker(102, ' '); + + line_breaker[0] = '\n'; + line_breaker[1] = line_breaker[101] = '\r'; + + std::ostringstream param_display; + + param_display << line_breaker << line_breaker << "########## Input Parameters ##########" + << line_breaker; + + param_display << "\tleaf_size: " << leaf_size << line_breaker; + param_display << "\tinput_pcd_dir: " << input_pcd_dir << line_breaker; + param_display << "\toutput_pcd: " << output_pcd << line_breaker; + param_display << "\tpoint_type: " << point_type << line_breaker; + param_display << "######################################" << line_breaker; + + RCLCPP_INFO(get_logger(), "%s", param_display.str().c_str()); + + if (point_type == "point_xyz") { + autoware::pointcloud_merger::PCDMerger pcd_merger_exe(get_logger()); + + pcd_merger_exe.setLeafSize(leaf_size); + pcd_merger_exe.setInput(input_pcd_dir); + pcd_merger_exe.setOutput(output_pcd); + + pcd_merger_exe.run(); + } else if (point_type == "point_xyzi") { + autoware::pointcloud_merger::PCDMerger pcd_merger_exe(get_logger()); + + pcd_merger_exe.setLeafSize(leaf_size); + pcd_merger_exe.setInput(input_pcd_dir); + pcd_merger_exe.setOutput(output_pcd); + + pcd_merger_exe.run(); + } + + rclcpp::shutdown(); +} + +} // namespace autoware::pointcloud_merger + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_merger::PointCloudMerger)