feat(pointcloud projection converter): add new package, pointcloud projection converter

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
+)

map/autoware_pointcloud_projection_converter/README.md

@@ -0,0 +1,39 @@
+# 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).

map/autoware_pointcloud_projection_converter/config/input.yaml

@@ -0,0 +1,3 @@
+projector_type: MGRS
+vertical_datum: WGS84
+mgrs_grid: 54SUE

map/autoware_pointcloud_projection_converter/config/output.yaml

@@ -0,0 +1,6 @@
+projector_type: TransverseMercator
+vertical_datum: WGS84
+map_origin:
+  latitude: 35.61739731
+  longitude: 139.7797546
+scale_factor: 0.9996

map/autoware_pointcloud_projection_converter/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_pointcloud_projection_converter</name>
+  <version>0.2.0</version>
+  <description>autoware_pointcloud_projection_converter</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">Anh Nguyen</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author>Koji Minoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>autoware_internal_debug_msgs</buildtool_depend>
+
+  <depend>geographiclib</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>yaml-cpp</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

map/autoware_pointcloud_projection_converter/src/converter_from_llh.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 "converter_from_llh.hpp"
+
+#include <iostream>
+#include <string>
+#include <utility>
+
+namespace autoware::pointcloud_projection_converter
+{
+
+ConverterFromLLH::ConverterFromLLH(const YAML::Node & config)
+{
+  projector_type_ = config["projector_type"].as<std::string>();
+
+  if (projector_type_ == "TransverseMercator") {
+    central_meridian_ = config["map_origin"]["longitude"].as<double>();
+
+    // 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<double>(),
+      config["map_origin"]["longitude"].as<double>(), x, y);
+    origin_xy_ = std::pair<double, double>(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

map/autoware_pointcloud_projection_converter/src/converter_from_llh.hpp

@@ -0,0 +1,49 @@
+// 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 CONVERTER_FROM_LLH_HPP_
+#define CONVERTER_FROM_LLH_HPP_
+
+#include "lat_lon_alt.hpp"
+
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/TransverseMercatorExact.hpp>
+
+#include <pcl/point_types.h>
+#include <yaml-cpp/yaml.h>
+
+#include <iostream>
+#include <string>
+#include <utility>
+
+namespace autoware::pointcloud_projection_converter
+{
+
+class ConverterFromLLH
+{
+public:
+  explicit ConverterFromLLH(const YAML::Node & config);
+  pcl::PointXYZI convert(const LatLonAlt & xyz);
+
+private:
+  std::string projector_type_;
+  std::pair<double, double> origin_xy_;
+  double central_meridian_;
+};
+
+}  // namespace autoware::pointcloud_projection_converter
+
+#endif  // CONVERTER_FROM_LLH_HPP_

map/autoware_pointcloud_projection_converter/src/converter_to_llh.cpp

@@ -0,0 +1,60 @@
+// 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_to_llh.hpp"
+
+#include <iomanip>
+#include <iostream>
+#include <string>
+
+namespace autoware::pointcloud_projection_converter
+{
+
+ConverterToLLH::ConverterToLLH(const YAML::Node & config)
+{
+  projector_type_ = config["projector_type"].as<std::string>();
+  if (projector_type_ == "MGRS") {
+    mgrs_grid_ = config["mgrs_grid"].as<std::string>();
+  }
+}
+
+LatLonAlt ConverterToLLH::convert(const pcl::PointXYZI & xyz)
+{
+  LatLonAlt llh;
+  if (projector_type_ == "MGRS") {
+    try {
+      int zone;
+      bool northern_hemisphere;
+      double mgrs_base_x, mgrs_base_y;
+      int prec = 8;
+      bool longpath = false;
+      GeographicLib::MGRS::Reverse(
+        mgrs_grid_, zone, northern_hemisphere, mgrs_base_x, mgrs_base_y, prec, longpath);
+
+      // Convert UTM to LLH
+      GeographicLib::UTMUPS::Reverse(
+        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) {
+      std::cerr << "Error: Could not convert from MGRS to UTM: " << e.what() << std::endl;
+      return LatLonAlt();
+    }
+  }
+  return llh;
+}
+
+}  // namespace autoware::pointcloud_projection_converter

map/autoware_pointcloud_projection_converter/src/converter_to_llh.hpp

@@ -0,0 +1,47 @@
+// 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 CONVERTER_TO_LLH_HPP_
+#define CONVERTER_TO_LLH_HPP_
+
+#include "lat_lon_alt.hpp"
+
+#include <GeographicLib/MGRS.hpp>
+
+#include <pcl/point_types.h>
+#include <yaml-cpp/yaml.h>
+
+#include <iostream>
+#include <string>
+
+namespace autoware::pointcloud_projection_converter
+{
+
+class ConverterToLLH
+{
+public:
+  explicit 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  // CONVERTER_TO_LLH_HPP_

map/autoware_pointcloud_projection_converter/src/lat_lon_alt.hpp

@@ -0,0 +1,32 @@
+// 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 LAT_LON_ALT_HPP_
+#define LAT_LON_ALT_HPP_
+
+namespace autoware::pointcloud_projection_converter
+{
+
+struct LatLonAlt
+{
+  double lat;
+  double lon;
+  double alt;
+};
+
+}  // namespace autoware::pointcloud_projection_converter
+
+#endif  // LAT_LON_ALT_HPP_