From 5353f41cef4bfea608400ec9227248849f5215aa Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 16 May 2025 17:49:02 +0900 Subject: [PATCH 01/17] add pointcloud projection converter Signed-off-by: Yamato Ando --- .../CMakeLists.txt | 41 +++++++++++ .../README.md | 35 ++++++++++ .../config/input.yaml | 3 + .../config/output.yaml | 4 ++ .../package.xml | 27 ++++++++ .../src/converter_from_llh.cpp | 64 +++++++++++++++++ .../src/converter_from_llh.hpp | 46 +++++++++++++ .../src/converter_to_llh.cpp | 58 ++++++++++++++++ .../src/converter_to_llh.hpp | 44 ++++++++++++ .../src/lat_lon_alt.hpp | 31 +++++++++ .../src/pcd_conversion.cpp | 69 +++++++++++++++++++ 11 files changed, 422 insertions(+) create mode 100644 map/autoware_pointcloud_projection_converter/CMakeLists.txt create mode 100644 map/autoware_pointcloud_projection_converter/README.md create mode 100644 map/autoware_pointcloud_projection_converter/config/input.yaml create mode 100644 map/autoware_pointcloud_projection_converter/config/output.yaml create mode 100644 map/autoware_pointcloud_projection_converter/package.xml create mode 100644 map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp create mode 100644 map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp create mode 100644 map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp create mode 100644 map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp create mode 100644 map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp create mode 100644 map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp diff --git a/map/autoware_pointcloud_projection_converter/CMakeLists.txt b/map/autoware_pointcloud_projection_converter/CMakeLists.txt new file mode 100644 index 000000000..b7f0dac13 --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.14) + +project(autoware_pointcloud_projection_converter) + +# Find required packages +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES NAMES Geographic) + +find_package(OpenMP REQUIRED) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + +find_package(PCL REQUIRED) +include_directories( + ${PCL_INCLUDE_DIRS} +) + +# Add the sub.cpp file as a library +add_library(converter_lib "src/converter_from_llh.cpp" "src/converter_to_llh.cpp") + +# Create executables +target_include_directories(converter_lib + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) +ament_auto_add_executable(pointcloud_projection_converter "src/pcd_conversion.cpp") + + +# Link the libraries +target_link_libraries(converter_lib yaml-cpp ${PCL_LIBRARIES} ${GeographicLib_LIBRARIES}) +target_link_libraries(pointcloud_projection_converter converter_lib yaml-cpp ${PCL_LIBRARIES} ${GeographicLib_LIBRARIES}) + +ament_auto_package(INSTALL_TO_SHARE + config +) diff --git a/map/autoware_pointcloud_projection_converter/README.md b/map/autoware_pointcloud_projection_converter/README.md new file mode 100644 index 000000000..122bca1a4 --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/README.md @@ -0,0 +1,35 @@ +# Point Cloud Projection Converter +This project includes a tool for converting point cloud data between different geodetic projection systems. The projection systems supported to convert from MGRS to Transverse Mercator. + +The conversion details (input and output projection types) are specified in two YAML configuration files. + +For example, to convert from MGRS to Transverse Mercator projection, you would use configuration files like this: + +```yaml +# input.yaml +projector_type: "MGRS" +mgrs_grid: "54SUE" +``` +```yaml +# output.yaml +projector_type: "TransverseMercator" +map_origin: + latitude: xx + longitude: yy +``` + +## Dependencies +- PCL (Point Cloud Library) 1.3 or higher +- yaml-cpp + +## Usage + +```bash +ros2 autoware_pointcloud_projection_converter pointcloud_projection_converter path_to_input_yaml path_to_output_yaml path_to_input_pcd_file path_to_output_pcd_file +``` +Replace `path_to_input_yaml`, `path_to_output_yaml`, `path_to_pointcloud_file`, and `path_to_output_pcd_file` with the paths to your input YAML configuration file, output YAML configuration file, and PCD file, respectively. + +## Special thanks + +This package reuses code from [kminoda/projection_converter](https://github.com/kminoda/projection_converter). +We have received permission to use it through [this inquiry](https://github.com/kminoda/projection_converter/issues/3). diff --git a/map/autoware_pointcloud_projection_converter/config/input.yaml b/map/autoware_pointcloud_projection_converter/config/input.yaml new file mode 100644 index 000000000..5f5e1b522 --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/config/input.yaml @@ -0,0 +1,3 @@ +# input.yaml +projector_type: "MGRS" +mgrs_grid: "54SUE" \ No newline at end of file diff --git a/map/autoware_pointcloud_projection_converter/config/output.yaml b/map/autoware_pointcloud_projection_converter/config/output.yaml new file mode 100644 index 000000000..0669990b2 --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/config/output.yaml @@ -0,0 +1,4 @@ +projector_type: "TransverseMercator" +map_origin: + latitude: 35.61739731 + longitude: 139.7797546 diff --git a/map/autoware_pointcloud_projection_converter/package.xml b/map/autoware_pointcloud_projection_converter/package.xml new file mode 100644 index 000000000..78ca964a2 --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/package.xml @@ -0,0 +1,27 @@ + + + + autoware_pointcloud_projection_converter + 0.2.0 + autoware_pointcloud_projection_converter + Yamato Ando + Taiki Yamada + Shintaro Sakoda + Anh Nguyen + Ryu Yamamoto + Masahiro Sakamoto + Apache License 2.0 + Koji Minoda + + ament_cmake_auto + autoware_cmake + autoware_internal_debug_msgs + + geographiclib + libpcl-all-dev + yaml-cpp + + + ament_cmake + + diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp new file mode 100644 index 000000000..8ea415746 --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp @@ -0,0 +1,64 @@ +// Copyright 2025 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. + +// The original code was written by Koji Minoda + +#include "converter_from_llh.hpp" + +namespace autoware::pointcloud_projection_converter +{ + +ConverterFromLLH::ConverterFromLLH(const YAML::Node &config) { + projector_type_ = config["projector_type"].as(); + + if (projector_type_ == "TransverseMercator") { + central_meridian_ = config["map_origin"]["longitude"].as(); + + // Calculate origin in Transverse Mercator coordinate + const GeographicLib::TransverseMercatorExact &proj = + GeographicLib::TransverseMercatorExact::UTM(); + double x, y; + proj.Forward(central_meridian_, + config["map_origin"]["latitude"].as(), + config["map_origin"]["longitude"].as(), x, y); + origin_xy_ = std::pair(x, y); + } +} + +pcl::PointXYZI ConverterFromLLH::convert(const LatLonAlt &llh) { + pcl::PointXYZI xyz; + if (projector_type_ == "TransverseMercator") { + const GeographicLib::TransverseMercatorExact &proj = + GeographicLib::TransverseMercatorExact::UTM(); + + // Variables to hold the results + double x, y; + + // Convert to transverse mercator coordinates + proj.Forward(central_meridian_, llh.lat, llh.lon, x, y); + xyz.x = x - origin_xy_.first; + xyz.y = y - origin_xy_.second; + xyz.z = llh.alt; + + } else { + std::cerr << "Only conversion to " + "TransverseMercator is supported currently.\n"; + std::cerr << "Not supported projector type: " << projector_type_ + << std::endl; + throw std::runtime_error(""); + } + return xyz; +} + +} // namespace autoware::pointcloud_projection_converter diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp new file mode 100644 index 000000000..be096e8ce --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp @@ -0,0 +1,46 @@ +// Copyright 2025 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. + +// The original code was written by Koji Minoda + +#ifndef POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_FROM_LLH_HPP +#define POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_FROM_LLH_HPP + +#include + +#include +#include +#include +#include +#include + +#include "lat_lon_alt.hpp" + +namespace autoware::pointcloud_projection_converter +{ + +class ConverterFromLLH { +public: + ConverterFromLLH(const YAML::Node &config); + pcl::PointXYZI convert(const LatLonAlt &xyz); + +private: + std::string projector_type_; + std::pair origin_xy_; + double central_meridian_; +}; + +} // namespace autoware::pointcloud_projection_converter + +#endif // POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_FROM_LLH_HPP diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp new file mode 100644 index 000000000..264e32b73 --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp @@ -0,0 +1,58 @@ +// Copyright 2025 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. + +// The original code was written by Koji Minoda + +#include +#include + +#include "converter_to_llh.hpp" + +namespace autoware::pointcloud_projection_converter +{ + +ConverterToLLH::ConverterToLLH(const YAML::Node &config) { + projector_type_ = config["projector_type"].as(); + if (projector_type_ == "MGRS") { + mgrs_grid_ = config["mgrs_grid"].as(); + } +} + +LatLonAlt ConverterToLLH::convert(const pcl::PointXYZI &xyz) { + LatLonAlt llh; + if (projector_type_ == "MGRS") { + try { + int zone; + bool northp; + double mgrs_base_x, mgrs_base_y; + int prec = 8; + bool longpath = false; + GeographicLib::MGRS::Reverse(mgrs_grid_, zone, northp, mgrs_base_x, + mgrs_base_y, prec, longpath); + + // Convert UTM to LLH + GeographicLib::UTMUPS::Reverse(zone, northp, xyz.x + mgrs_base_x, + xyz.y + mgrs_base_y, llh.lat, llh.lon); + + llh.alt = xyz.z; + } catch (const std::exception &e) { + std::cerr << "Error: Could not convert from MGRS to UTM: " << e.what() + << std::endl; + return LatLonAlt(); + } + } + return llh; +} + +} // namespace autoware::pointcloud_projection_converter diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp new file mode 100644 index 000000000..58664ca3c --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp @@ -0,0 +1,44 @@ +// Copyright 2025 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. + +// The original code was written by Koji Minoda + +#ifndef POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_TO_LLH_HPP +#define POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_TO_LLH_HPP + +#include + +#include +#include +#include + +#include "lat_lon_alt.hpp" + +namespace autoware::pointcloud_projection_converter +{ + +class ConverterToLLH { +public: + ConverterToLLH(const YAML::Node &config); + LatLonAlt convert(const pcl::PointXYZI &llh); + +private: + std::string projector_type_; + std::string mgrs_grid_; + double lat_, lon_; +}; + +} // namespace autoware::pointcloud_projection_converter + +#endif // POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_TO_LLH_HPP diff --git a/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp b/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp new file mode 100644 index 000000000..e57055ead --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp @@ -0,0 +1,31 @@ +// Copyright 2025 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. + +// The original code was written by Koji Minoda + +#ifndef POINTCLOUD_PROJECTION_CONVERTER__LAT_LON_ALT_HPP +#define POINTCLOUD_PROJECTION_CONVERTER__LAT_LON_ALT_HPP + +namespace autoware::pointcloud_projection_converter +{ + +struct LatLonAlt { + double lat; + double lon; + double alt; +}; + +} // namespace autoware::pointcloud_projection_converter + +#endif // POINTCLOUD_PROJECTION_CONVERTER__LAT_LON_ALT_HPP diff --git a/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp b/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp new file mode 100644 index 000000000..b7584a80e --- /dev/null +++ b/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp @@ -0,0 +1,69 @@ +// Copyright 2025 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. + +// The original code was written by Koji Minoda + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "converter_from_llh.hpp" +#include "converter_to_llh.hpp" +#include "lat_lon_alt.hpp" + +int main(int argc, char **argv) { + if (argc != 5) { + std::cerr << "Usage: ros2 run autoware_pointcloud_projection_converter pointcloud_projection_converter input_yaml output_yaml " + "input_pcd output_pcd" << std::endl; + std::exit(EXIT_FAILURE); + } + + // Parse YAML configuration files + YAML::Node input_config = YAML::LoadFile(argv[1]); + YAML::Node output_config = YAML::LoadFile(argv[2]); + + // Load point cloud data from file + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(argv[3], *cloud) == -1) { + std::cerr << "Couldn't read file " << argv[3] << std::endl; + std::exit(EXIT_FAILURE); + } + + // Define converters + autoware::pointcloud_projection_converter::ConverterToLLH to_llh(input_config); + autoware::pointcloud_projection_converter::ConverterFromLLH from_llh(output_config); + + // Convert points + size_t n_points = cloud->points.size(); + +#pragma omp parallel for + for (size_t i = 0; i < n_points; ++i) { + auto &point = cloud->points[i]; + autoware::pointcloud_projection_converter::LatLonAlt llh = to_llh.convert(point); + point = from_llh.convert(llh); + } + + // Save converted point cloud to file + pcl::io::savePCDFileBinary(argv[4], *cloud); + + std::cout << "Point cloud projection conversion completed successfully" << std::endl; + + return 0; +} From a1d70f831e9c547cb4d6606cee1b7736770a07dc Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 16 May 2025 17:50:55 +0900 Subject: [PATCH 02/17] add newline Signed-off-by: Yamato Ando --- map/autoware_pointcloud_projection_converter/config/input.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/autoware_pointcloud_projection_converter/config/input.yaml b/map/autoware_pointcloud_projection_converter/config/input.yaml index 5f5e1b522..e86a43901 100644 --- a/map/autoware_pointcloud_projection_converter/config/input.yaml +++ b/map/autoware_pointcloud_projection_converter/config/input.yaml @@ -1,3 +1,3 @@ # input.yaml projector_type: "MGRS" -mgrs_grid: "54SUE" \ No newline at end of file +mgrs_grid: "54SUE" From f2edbf92789f7eee12bf435ce0f8b843ab2f089c Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 16 May 2025 18:14:52 +0900 Subject: [PATCH 03/17] update config file Signed-off-by: Yamato Ando --- .../config/input.yaml | 6 +++--- .../config/output.yaml | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/map/autoware_pointcloud_projection_converter/config/input.yaml b/map/autoware_pointcloud_projection_converter/config/input.yaml index e86a43901..5446de489 100644 --- a/map/autoware_pointcloud_projection_converter/config/input.yaml +++ b/map/autoware_pointcloud_projection_converter/config/input.yaml @@ -1,3 +1,3 @@ -# input.yaml -projector_type: "MGRS" -mgrs_grid: "54SUE" +projector_type: MGRS +vertical_datum: WGS84 +mgrs_grid: 54SUE diff --git a/map/autoware_pointcloud_projection_converter/config/output.yaml b/map/autoware_pointcloud_projection_converter/config/output.yaml index 0669990b2..b027ae6dd 100644 --- a/map/autoware_pointcloud_projection_converter/config/output.yaml +++ b/map/autoware_pointcloud_projection_converter/config/output.yaml @@ -1,4 +1,6 @@ -projector_type: "TransverseMercator" +projector_type: TransverseMercator +vertical_datum: WGS84 map_origin: latitude: 35.61739731 longitude: 139.7797546 +scale_factor: 0.9996 From 41514d3d44e62553c2c71e57daf8c74c48def5c0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 16 May 2025 09:31:16 +0000 Subject: [PATCH 04/17] style(pre-commit): autofix --- .../CMakeLists.txt | 2 +- .../README.md | 4 +++ .../src/converter_from_llh.cpp | 23 ++++++++-------- .../src/converter_from_llh.hpp | 20 +++++++------- .../src/converter_to_llh.cpp | 23 ++++++++-------- .../src/converter_to_llh.hpp | 18 +++++++------ .../src/lat_lon_alt.hpp | 9 ++++--- .../src/pcd_conversion.cpp | 26 +++++++++++-------- 8 files changed, 70 insertions(+), 55 deletions(-) diff --git a/map/autoware_pointcloud_projection_converter/CMakeLists.txt b/map/autoware_pointcloud_projection_converter/CMakeLists.txt index b7f0dac13..1a8d381d5 100644 --- a/map/autoware_pointcloud_projection_converter/CMakeLists.txt +++ b/map/autoware_pointcloud_projection_converter/CMakeLists.txt @@ -36,6 +36,6 @@ ament_auto_add_executable(pointcloud_projection_converter "src/pcd_conversion.cp target_link_libraries(converter_lib yaml-cpp ${PCL_LIBRARIES} ${GeographicLib_LIBRARIES}) target_link_libraries(pointcloud_projection_converter converter_lib yaml-cpp ${PCL_LIBRARIES} ${GeographicLib_LIBRARIES}) -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package(INSTALL_TO_SHARE config ) diff --git a/map/autoware_pointcloud_projection_converter/README.md b/map/autoware_pointcloud_projection_converter/README.md index 122bca1a4..342eeb609 100644 --- a/map/autoware_pointcloud_projection_converter/README.md +++ b/map/autoware_pointcloud_projection_converter/README.md @@ -1,4 +1,5 @@ # Point Cloud Projection Converter + This project includes a tool for converting point cloud data between different geodetic projection systems. The projection systems supported to convert from MGRS to Transverse Mercator. The conversion details (input and output projection types) are specified in two YAML configuration files. @@ -10,6 +11,7 @@ For example, to convert from MGRS to Transverse Mercator projection, you would u projector_type: "MGRS" mgrs_grid: "54SUE" ``` + ```yaml # output.yaml projector_type: "TransverseMercator" @@ -19,6 +21,7 @@ map_origin: ``` ## Dependencies + - PCL (Point Cloud Library) 1.3 or higher - yaml-cpp @@ -27,6 +30,7 @@ map_origin: ```bash ros2 autoware_pointcloud_projection_converter pointcloud_projection_converter path_to_input_yaml path_to_output_yaml path_to_input_pcd_file path_to_output_pcd_file ``` + Replace `path_to_input_yaml`, `path_to_output_yaml`, `path_to_pointcloud_file`, and `path_to_output_pcd_file` with the paths to your input YAML configuration file, output YAML configuration file, and PCD file, respectively. ## Special thanks diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp index 8ea415746..7833872c0 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp @@ -19,28 +19,30 @@ namespace autoware::pointcloud_projection_converter { -ConverterFromLLH::ConverterFromLLH(const YAML::Node &config) { +ConverterFromLLH::ConverterFromLLH(const YAML::Node & config) +{ projector_type_ = config["projector_type"].as(); if (projector_type_ == "TransverseMercator") { central_meridian_ = config["map_origin"]["longitude"].as(); // Calculate origin in Transverse Mercator coordinate - const GeographicLib::TransverseMercatorExact &proj = - GeographicLib::TransverseMercatorExact::UTM(); + const GeographicLib::TransverseMercatorExact & proj = + GeographicLib::TransverseMercatorExact::UTM(); double x, y; - proj.Forward(central_meridian_, - config["map_origin"]["latitude"].as(), - config["map_origin"]["longitude"].as(), x, y); + proj.Forward( + central_meridian_, config["map_origin"]["latitude"].as(), + config["map_origin"]["longitude"].as(), x, y); origin_xy_ = std::pair(x, y); } } -pcl::PointXYZI ConverterFromLLH::convert(const LatLonAlt &llh) { +pcl::PointXYZI ConverterFromLLH::convert(const LatLonAlt & llh) +{ pcl::PointXYZI xyz; if (projector_type_ == "TransverseMercator") { - const GeographicLib::TransverseMercatorExact &proj = - GeographicLib::TransverseMercatorExact::UTM(); + const GeographicLib::TransverseMercatorExact & proj = + GeographicLib::TransverseMercatorExact::UTM(); // Variables to hold the results double x, y; @@ -54,8 +56,7 @@ pcl::PointXYZI ConverterFromLLH::convert(const LatLonAlt &llh) { } else { std::cerr << "Only conversion to " "TransverseMercator is supported currently.\n"; - std::cerr << "Not supported projector type: " << projector_type_ - << std::endl; + std::cerr << "Not supported projector type: " << projector_type_ << std::endl; throw std::runtime_error(""); } return xyz; diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp index be096e8ce..43485445f 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp @@ -14,26 +14,28 @@ // The original code was written by Koji Minoda -#ifndef POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_FROM_LLH_HPP -#define POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_FROM_LLH_HPP +#ifndef CONVERTER_FROM_LLH_HPP_ +#define CONVERTER_FROM_LLH_HPP_ -#include +#include "lat_lon_alt.hpp" #include #include -#include + #include #include -#include "lat_lon_alt.hpp" +#include +#include namespace autoware::pointcloud_projection_converter { -class ConverterFromLLH { +class ConverterFromLLH +{ public: - ConverterFromLLH(const YAML::Node &config); - pcl::PointXYZI convert(const LatLonAlt &xyz); + ConverterFromLLH(const YAML::Node & config); + pcl::PointXYZI convert(const LatLonAlt & xyz); private: std::string projector_type_; @@ -43,4 +45,4 @@ class ConverterFromLLH { } // namespace autoware::pointcloud_projection_converter -#endif // POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_FROM_LLH_HPP +#endif // CONVERTER_FROM_LLH_HPP_ diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp index 264e32b73..2e9abf87e 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp @@ -14,22 +14,24 @@ // The original code was written by Koji Minoda +#include "converter_to_llh.hpp" + #include #include -#include "converter_to_llh.hpp" - namespace autoware::pointcloud_projection_converter { -ConverterToLLH::ConverterToLLH(const YAML::Node &config) { +ConverterToLLH::ConverterToLLH(const YAML::Node & config) +{ projector_type_ = config["projector_type"].as(); if (projector_type_ == "MGRS") { mgrs_grid_ = config["mgrs_grid"].as(); } } -LatLonAlt ConverterToLLH::convert(const pcl::PointXYZI &xyz) { +LatLonAlt ConverterToLLH::convert(const pcl::PointXYZI & xyz) +{ LatLonAlt llh; if (projector_type_ == "MGRS") { try { @@ -38,17 +40,16 @@ LatLonAlt ConverterToLLH::convert(const pcl::PointXYZI &xyz) { double mgrs_base_x, mgrs_base_y; int prec = 8; bool longpath = false; - GeographicLib::MGRS::Reverse(mgrs_grid_, zone, northp, mgrs_base_x, - mgrs_base_y, prec, longpath); + GeographicLib::MGRS::Reverse( + mgrs_grid_, zone, northp, mgrs_base_x, mgrs_base_y, prec, longpath); // Convert UTM to LLH - GeographicLib::UTMUPS::Reverse(zone, northp, xyz.x + mgrs_base_x, - xyz.y + mgrs_base_y, llh.lat, llh.lon); + GeographicLib::UTMUPS::Reverse( + zone, northp, xyz.x + mgrs_base_x, xyz.y + mgrs_base_y, llh.lat, llh.lon); llh.alt = xyz.z; - } catch (const std::exception &e) { - std::cerr << "Error: Could not convert from MGRS to UTM: " << e.what() - << std::endl; + } catch (const std::exception & e) { + std::cerr << "Error: Could not convert from MGRS to UTM: " << e.what() << std::endl; return LatLonAlt(); } } diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp index 58664ca3c..bdb43fee6 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp @@ -14,24 +14,26 @@ // The original code was written by Koji Minoda -#ifndef POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_TO_LLH_HPP -#define POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_TO_LLH_HPP +#ifndef CONVERTER_TO_LLH_HPP_ +#define CONVERTER_TO_LLH_HPP_ -#include +#include "lat_lon_alt.hpp" #include + #include #include -#include "lat_lon_alt.hpp" +#include namespace autoware::pointcloud_projection_converter { -class ConverterToLLH { +class ConverterToLLH +{ public: - ConverterToLLH(const YAML::Node &config); - LatLonAlt convert(const pcl::PointXYZI &llh); + ConverterToLLH(const YAML::Node & config); + LatLonAlt convert(const pcl::PointXYZI & llh); private: std::string projector_type_; @@ -41,4 +43,4 @@ class ConverterToLLH { } // namespace autoware::pointcloud_projection_converter -#endif // POINTCLOUD_PROJECTION_CONVERTER__CONVERTER_TO_LLH_HPP +#endif // CONVERTER_TO_LLH_HPP_ diff --git a/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp b/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp index e57055ead..fba8e0fe8 100644 --- a/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp +++ b/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp @@ -14,13 +14,14 @@ // The original code was written by Koji Minoda -#ifndef POINTCLOUD_PROJECTION_CONVERTER__LAT_LON_ALT_HPP -#define POINTCLOUD_PROJECTION_CONVERTER__LAT_LON_ALT_HPP +#ifndef LAT_LON_ALT_HPP_ +#define LAT_LON_ALT_HPP_ namespace autoware::pointcloud_projection_converter { -struct LatLonAlt { +struct LatLonAlt +{ double lat; double lon; double alt; @@ -28,4 +29,4 @@ struct LatLonAlt { } // namespace autoware::pointcloud_projection_converter -#endif // POINTCLOUD_PROJECTION_CONVERTER__LAT_LON_ALT_HPP +#endif // LAT_LON_ALT_HPP_ diff --git a/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp b/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp index b7584a80e..a58ff417e 100644 --- a/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp +++ b/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp @@ -14,24 +14,28 @@ // The original code was written by Koji Minoda -#include -#include -#include -#include +#include "converter_from_llh.hpp" +#include "converter_to_llh.hpp" +#include "lat_lon_alt.hpp" #include + #include #include #include -#include "converter_from_llh.hpp" -#include "converter_to_llh.hpp" -#include "lat_lon_alt.hpp" +#include +#include +#include +#include -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ if (argc != 5) { - std::cerr << "Usage: ros2 run autoware_pointcloud_projection_converter pointcloud_projection_converter input_yaml output_yaml " - "input_pcd output_pcd" << std::endl; + std::cerr << "Usage: ros2 run autoware_pointcloud_projection_converter " + "pointcloud_projection_converter input_yaml output_yaml " + "input_pcd output_pcd" + << std::endl; std::exit(EXIT_FAILURE); } @@ -55,7 +59,7 @@ int main(int argc, char **argv) { #pragma omp parallel for for (size_t i = 0; i < n_points; ++i) { - auto &point = cloud->points[i]; + auto & point = cloud->points[i]; autoware::pointcloud_projection_converter::LatLonAlt llh = to_llh.convert(point); point = from_llh.convert(llh); } From b4d54593977053859743e227f900ea9f49b2d184 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 29 May 2025 17:49:16 +0900 Subject: [PATCH 05/17] fix variable name Signed-off-by: Yamato Ando --- .../src/converter_to_llh.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp index 2e9abf87e..8fc1465e7 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp @@ -36,16 +36,16 @@ LatLonAlt ConverterToLLH::convert(const pcl::PointXYZI & xyz) if (projector_type_ == "MGRS") { try { int zone; - bool northp; + bool northern_hemisphere; double mgrs_base_x, mgrs_base_y; int prec = 8; bool longpath = false; GeographicLib::MGRS::Reverse( - mgrs_grid_, zone, northp, mgrs_base_x, mgrs_base_y, prec, longpath); + mgrs_grid_, zone, northern_hemisphere, mgrs_base_x, mgrs_base_y, prec, longpath); // Convert UTM to LLH GeographicLib::UTMUPS::Reverse( - zone, northp, xyz.x + mgrs_base_x, xyz.y + mgrs_base_y, llh.lat, llh.lon); + zone, northern_hemisphere, xyz.x + mgrs_base_x, xyz.y + mgrs_base_y, llh.lat, llh.lon); llh.alt = xyz.z; } catch (const std::exception & e) { From b4c7541d5066ba5df385571a6884f5d79cbb5c8e Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 29 May 2025 17:53:19 +0900 Subject: [PATCH 06/17] fix precommit error Signed-off-by: Yamato Ando --- .../src/converter_from_llh.cpp | 4 ++++ .../src/converter_from_llh.hpp | 3 ++- .../src/converter_to_llh.cpp | 1 + .../src/converter_to_llh.hpp | 3 ++- 4 files changed, 9 insertions(+), 2 deletions(-) diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp index 7833872c0..65b8b478b 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.cpp @@ -16,6 +16,10 @@ #include "converter_from_llh.hpp" +#include +#include +#include + namespace autoware::pointcloud_projection_converter { diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp index 43485445f..8f46664dc 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp @@ -27,6 +27,7 @@ #include #include +#include namespace autoware::pointcloud_projection_converter { @@ -34,7 +35,7 @@ namespace autoware::pointcloud_projection_converter class ConverterFromLLH { public: - ConverterFromLLH(const YAML::Node & config); + explicit ConverterFromLLH(const YAML::Node & config); pcl::PointXYZI convert(const LatLonAlt & xyz); private: diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp index 8fc1465e7..60555a0e6 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace autoware::pointcloud_projection_converter { diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp index bdb43fee6..4afcc58c5 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp @@ -25,6 +25,7 @@ #include #include +#include namespace autoware::pointcloud_projection_converter { @@ -32,7 +33,7 @@ namespace autoware::pointcloud_projection_converter class ConverterToLLH { public: - ConverterToLLH(const YAML::Node & config); + explicit ConverterToLLH(const YAML::Node & config); LatLonAlt convert(const pcl::PointXYZI & llh); private: From 907649fef33329ce44555c72012b7196cd83af34 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 29 May 2025 08:55:23 +0000 Subject: [PATCH 07/17] style(pre-commit): autofix --- .../src/converter_from_llh.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp index 8f46664dc..165606090 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp @@ -26,8 +26,8 @@ #include #include -#include #include +#include namespace autoware::pointcloud_projection_converter { From cd6c687a4930c8e603d05a155818c4a8e2fc9139 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:08:55 +0900 Subject: [PATCH 08/17] add constexpr Signed-off-by: Yamato Ando --- .../src/converter_to_llh.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp index 60555a0e6..207c00319 100644 --- a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp +++ b/map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp @@ -40,7 +40,7 @@ LatLonAlt ConverterToLLH::convert(const pcl::PointXYZI & xyz) bool northern_hemisphere; double mgrs_base_x, mgrs_base_y; int prec = 8; - bool longpath = false; + constexpr bool longpath = false; GeographicLib::MGRS::Reverse( mgrs_grid_, zone, northern_hemisphere, mgrs_base_x, mgrs_base_y, prec, longpath); From dec76c307b25c23d45fdb8f2de44b991c961f09d Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:09:35 +0900 Subject: [PATCH 09/17] add const Signed-off-by: Yamato Ando --- .../src/pcd_conversion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp b/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp index a58ff417e..455f4b336 100644 --- a/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp +++ b/map/autoware_pointcloud_projection_converter/src/pcd_conversion.cpp @@ -55,7 +55,7 @@ int main(int argc, char ** argv) autoware::pointcloud_projection_converter::ConverterFromLLH from_llh(output_config); // Convert points - size_t n_points = cloud->points.size(); + const size_t n_points = cloud->points.size(); #pragma omp parallel for for (size_t i = 0; i < n_points; ++i) { From d40da313f9a2be4794b36ccc85f8596457387451 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:20:30 +0900 Subject: [PATCH 10/17] add cmake option Signed-off-by: Yamato Ando --- map/autoware_pointcloud_projection_converter/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/map/autoware_pointcloud_projection_converter/CMakeLists.txt b/map/autoware_pointcloud_projection_converter/CMakeLists.txt index 1a8d381d5..42b867cc1 100644 --- a/map/autoware_pointcloud_projection_converter/CMakeLists.txt +++ b/map/autoware_pointcloud_projection_converter/CMakeLists.txt @@ -6,6 +6,12 @@ project(autoware_pointcloud_projection_converter) find_package(autoware_cmake REQUIRED) autoware_package() +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_package(PkgConfig) find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h PATH_SUFFIXES GeographicLib From c5d1ce3145a7b3c11cfc34aecffbf8fd5a7a1a1c Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:22:41 +0900 Subject: [PATCH 11/17] update cmakelist Signed-off-by: Yamato Ando --- .../CMakeLists.txt | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/map/autoware_pointcloud_projection_converter/CMakeLists.txt b/map/autoware_pointcloud_projection_converter/CMakeLists.txt index 42b867cc1..2adc81d30 100644 --- a/map/autoware_pointcloud_projection_converter/CMakeLists.txt +++ b/map/autoware_pointcloud_projection_converter/CMakeLists.txt @@ -27,20 +27,11 @@ include_directories( ${PCL_INCLUDE_DIRS} ) -# Add the sub.cpp file as a library -add_library(converter_lib "src/converter_from_llh.cpp" "src/converter_to_llh.cpp") +ament_auto_add_library(converter_lib src/converter_from_llh.cpp src/converter_to_llh.cpp) +target_link_libraries(converter_lib ${GeographicLib_LIBRARIES} ${PCL_LIBRARIES}) -# Create executables -target_include_directories(converter_lib - SYSTEM PUBLIC - ${PCL_INCLUDE_DIRS} -) -ament_auto_add_executable(pointcloud_projection_converter "src/pcd_conversion.cpp") - - -# Link the libraries -target_link_libraries(converter_lib yaml-cpp ${PCL_LIBRARIES} ${GeographicLib_LIBRARIES}) -target_link_libraries(pointcloud_projection_converter converter_lib yaml-cpp ${PCL_LIBRARIES} ${GeographicLib_LIBRARIES}) +ament_auto_add_executable(pointcloud_projection_converter src/pcd_conversion.cpp) +target_link_libraries(pointcloud_projection_converter converter_lib yaml-cpp) ament_auto_package(INSTALL_TO_SHARE config From 1083c5ea8a1f66c7395712851faf488c715b5f28 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:23:15 +0900 Subject: [PATCH 12/17] fix typo in readme Signed-off-by: Yamato Ando --- map/autoware_pointcloud_projection_converter/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/autoware_pointcloud_projection_converter/README.md b/map/autoware_pointcloud_projection_converter/README.md index 342eeb609..5501b5bd4 100644 --- a/map/autoware_pointcloud_projection_converter/README.md +++ b/map/autoware_pointcloud_projection_converter/README.md @@ -28,7 +28,7 @@ map_origin: ## Usage ```bash -ros2 autoware_pointcloud_projection_converter pointcloud_projection_converter path_to_input_yaml path_to_output_yaml path_to_input_pcd_file path_to_output_pcd_file +ros2 run autoware_pointcloud_projection_converter pointcloud_projection_converter path_to_input_yaml path_to_output_yaml path_to_input_pcd_file path_to_output_pcd_file ``` Replace `path_to_input_yaml`, `path_to_output_yaml`, `path_to_pointcloud_file`, and `path_to_output_pcd_file` with the paths to your input YAML configuration file, output YAML configuration file, and PCD file, respectively. From 7c716c6edca3c755dc743b44867c560436e9e6b3 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:24:07 +0900 Subject: [PATCH 13/17] add dependanices in readme Signed-off-by: Yamato Ando --- map/autoware_pointcloud_projection_converter/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/map/autoware_pointcloud_projection_converter/README.md b/map/autoware_pointcloud_projection_converter/README.md index 5501b5bd4..601cf1ff8 100644 --- a/map/autoware_pointcloud_projection_converter/README.md +++ b/map/autoware_pointcloud_projection_converter/README.md @@ -24,6 +24,8 @@ map_origin: - PCL (Point Cloud Library) 1.3 or higher - yaml-cpp +- GeographicLib +- OpenMP ## Usage From 1db612925dfaae55e6ea565546c03d9c030c3c6a Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:25:52 +0900 Subject: [PATCH 14/17] add pcl_conversions Signed-off-by: Yamato Ando --- map/autoware_pointcloud_projection_converter/CMakeLists.txt | 5 ----- map/autoware_pointcloud_projection_converter/package.xml | 1 + 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/map/autoware_pointcloud_projection_converter/CMakeLists.txt b/map/autoware_pointcloud_projection_converter/CMakeLists.txt index 2adc81d30..d85bde3d1 100644 --- a/map/autoware_pointcloud_projection_converter/CMakeLists.txt +++ b/map/autoware_pointcloud_projection_converter/CMakeLists.txt @@ -22,11 +22,6 @@ find_library(GeographicLib_LIBRARIES NAMES Geographic) find_package(OpenMP REQUIRED) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -find_package(PCL REQUIRED) -include_directories( - ${PCL_INCLUDE_DIRS} -) - ament_auto_add_library(converter_lib src/converter_from_llh.cpp src/converter_to_llh.cpp) target_link_libraries(converter_lib ${GeographicLib_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/map/autoware_pointcloud_projection_converter/package.xml b/map/autoware_pointcloud_projection_converter/package.xml index 78ca964a2..0615eb136 100644 --- a/map/autoware_pointcloud_projection_converter/package.xml +++ b/map/autoware_pointcloud_projection_converter/package.xml @@ -19,6 +19,7 @@ geographiclib libpcl-all-dev + pcl_conversions yaml-cpp From 3faf43ec8874726ee82a4f7edf68f75e9bf6d37b Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:33:41 +0900 Subject: [PATCH 15/17] add libomp-dev Signed-off-by: Yamato Ando --- map/autoware_pointcloud_projection_converter/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/map/autoware_pointcloud_projection_converter/package.xml b/map/autoware_pointcloud_projection_converter/package.xml index 0615eb136..7bf845d6a 100644 --- a/map/autoware_pointcloud_projection_converter/package.xml +++ b/map/autoware_pointcloud_projection_converter/package.xml @@ -21,6 +21,7 @@ libpcl-all-dev pcl_conversions yaml-cpp + libomp-dev ament_cmake From b82d6c1cefbc1140ded0251a6c57ed982eb0289c Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 4 Jun 2025 19:37:33 +0900 Subject: [PATCH 16/17] move .hpp files Signed-off-by: Yamato Ando --- map/autoware_pointcloud_projection_converter/CMakeLists.txt | 2 ++ .../src/{ => include}/converter_from_llh.hpp | 0 .../src/{ => include}/converter_to_llh.hpp | 0 .../src/{ => include}/lat_lon_alt.hpp | 0 4 files changed, 2 insertions(+) rename map/autoware_pointcloud_projection_converter/src/{ => include}/converter_from_llh.hpp (100%) rename map/autoware_pointcloud_projection_converter/src/{ => include}/converter_to_llh.hpp (100%) rename map/autoware_pointcloud_projection_converter/src/{ => include}/lat_lon_alt.hpp (100%) diff --git a/map/autoware_pointcloud_projection_converter/CMakeLists.txt b/map/autoware_pointcloud_projection_converter/CMakeLists.txt index d85bde3d1..026b18b81 100644 --- a/map/autoware_pointcloud_projection_converter/CMakeLists.txt +++ b/map/autoware_pointcloud_projection_converter/CMakeLists.txt @@ -22,6 +22,8 @@ find_library(GeographicLib_LIBRARIES NAMES Geographic) find_package(OpenMP REQUIRED) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +include_directories(src/include) + ament_auto_add_library(converter_lib src/converter_from_llh.cpp src/converter_to_llh.cpp) target_link_libraries(converter_lib ${GeographicLib_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp b/map/autoware_pointcloud_projection_converter/src/include/converter_from_llh.hpp similarity index 100% rename from map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp rename to map/autoware_pointcloud_projection_converter/src/include/converter_from_llh.hpp diff --git a/map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp b/map/autoware_pointcloud_projection_converter/src/include/converter_to_llh.hpp similarity index 100% rename from map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp rename to map/autoware_pointcloud_projection_converter/src/include/converter_to_llh.hpp diff --git a/map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp b/map/autoware_pointcloud_projection_converter/src/include/lat_lon_alt.hpp similarity index 100% rename from map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp rename to map/autoware_pointcloud_projection_converter/src/include/lat_lon_alt.hpp From eb28ecb0e95facdeb9ec824a8577cd859c327573 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 4 Jun 2025 10:40:43 +0000 Subject: [PATCH 17/17] style(pre-commit): autofix --- map/autoware_pointcloud_projection_converter/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/autoware_pointcloud_projection_converter/package.xml b/map/autoware_pointcloud_projection_converter/package.xml index 7bf845d6a..af436a5d3 100644 --- a/map/autoware_pointcloud_projection_converter/package.xml +++ b/map/autoware_pointcloud_projection_converter/package.xml @@ -18,10 +18,10 @@ autoware_internal_debug_msgs geographiclib + libomp-dev libpcl-all-dev pcl_conversions yaml-cpp - libomp-dev ament_cmake