feat: port autoware_geography_utils from autoware.universe (#100)

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: Yutaka Kondo <[email protected]>

common/autoware_geography_utils/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_geography_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# GeographicLib
+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)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/height.cpp
+  src/projection.cpp
+  src/lanelet2_projector.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${GeographicLib_LIBRARIES}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files})
+
+  target_link_libraries(test_${PROJECT_NAME}
+  ${PROJECT_NAME}
+  )
+endif()
+
+ament_auto_package()

common/autoware_geography_utils/README.md

@@ -0,0 +1,5 @@
+# geography_utils
+
+## Purpose
+
+This package contains geography-related functions used by other packages, so please refer to them as needed.

common/autoware_geography_utils/include/autoware/geography_utils/height.hpp

@@ -0,0 +1,33 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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__GEOGRAPHY_UTILS__HEIGHT_HPP_
+#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_
+
+#include <string>
+
+namespace autoware::geography_utils
+{
+
+typedef double (*HeightConversionFunction)(
+  const double height, const double latitude, const double longitude);
+double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude);
+double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude);
+double convert_height(
+  const double height, const double latitude, const double longitude,
+  const std::string & source_vertical_datum, const std::string & target_vertical_datum);
+
+}  // namespace autoware::geography_utils
+
+#endif  // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_

common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp

@@ -0,0 +1,32 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
+#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
+
+#include <autoware_map_msgs/msg/map_projector_info.hpp>
+
+#include <lanelet2_io/Projection.h>
+
+#include <memory>
+
+namespace autoware::geography_utils
+{
+using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo;
+
+std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info);
+
+}  // namespace autoware::geography_utils
+
+#endif  // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_

common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp

@@ -0,0 +1,33 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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__GEOGRAPHY_UTILS__PROJECTION_HPP_
+#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_
+
+#include <autoware_map_msgs/msg/map_projector_info.hpp>
+#include <geographic_msgs/msg/geo_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
+
+namespace autoware::geography_utils
+{
+using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo;
+using GeoPoint = geographic_msgs::msg::GeoPoint;
+using LocalPoint = geometry_msgs::msg::Point;
+
+LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info);
+GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info);
+
+}  // namespace autoware::geography_utils
+
+#endif  // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_

common/autoware_geography_utils/package.xml

@@ -0,0 +1,33 @@
+<?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_geography_utils</name>
+  <version>0.0.0</version>
+  <description>The autoware_geography_utils package</description>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">NGUYEN Viet Anh</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <maintainer email="[email protected]">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Koji Minoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_lanelet2_extension</depend>
+  <depend>autoware_map_msgs</depend>
+  <depend>geographic_msgs</depend>
+  <depend>geographiclib</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_io</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/autoware_geography_utils/src/height.cpp

@@ -0,0 +1,63 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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 <GeographicLib/Geoid.hpp>
+#include <autoware/geography_utils/height.hpp>
+
+#include <map>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+namespace autoware::geography_utils
+{
+
+double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude)
+{
+  GeographicLib::Geoid egm2008("egm2008-1");
+  // cSpell: ignore ELLIPSOIDTOGEOID
+  return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID);
+}
+
+double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude)
+{
+  GeographicLib::Geoid egm2008("egm2008-1");
+  // cSpell: ignore GEOIDTOELLIPSOID
+  return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID);
+}
+
+double convert_height(
+  const double height, const double latitude, const double longitude,
+  const std::string & source_vertical_datum, const std::string & target_vertical_datum)
+{
+  if (source_vertical_datum == target_vertical_datum) {
+    return height;
+  }
+  std::map<std::pair<std::string, std::string>, HeightConversionFunction> conversion_map;
+  conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008;
+  conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84;
+
+  auto key = std::make_pair(source_vertical_datum, target_vertical_datum);
+  if (conversion_map.find(key) != conversion_map.end()) {
+    return conversion_map[key](height, latitude, longitude);
+  } else {
+    std::string error_message =
+      "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " +
+      std::string(target_vertical_datum.c_str());
+
+    throw std::invalid_argument(error_message);
+  }
+}
+
+}  // namespace autoware::geography_utils

common/autoware_geography_utils/src/lanelet2_projector.cpp

@@ -0,0 +1,54 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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 <GeographicLib/Geoid.hpp>
+#include <autoware/geography_utils/lanelet2_projector.hpp>
+#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
+#include <autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp>
+
+#include <lanelet2_projection/UTM.h>
+
+namespace autoware::geography_utils
+{
+
+std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info)
+{
+  if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
+    lanelet::GPSPoint position{
+      projector_info.map_origin.latitude, projector_info.map_origin.longitude,
+      projector_info.map_origin.altitude};
+    lanelet::Origin origin{position};
+    lanelet::projection::UtmProjector projector{origin};
+    return std::make_unique<lanelet::projection::UtmProjector>(projector);
+
+  } else if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    lanelet::projection::MGRSProjector projector{};
+    projector.setMGRSCode(projector_info.mgrs_grid);
+    return std::make_unique<lanelet::projection::MGRSProjector>(projector);
+
+  } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) {
+    lanelet::GPSPoint position{
+      projector_info.map_origin.latitude, projector_info.map_origin.longitude,
+      projector_info.map_origin.altitude};
+    lanelet::Origin origin{position};
+    lanelet::projection::TransverseMercatorProjector projector{origin};
+    return std::make_unique<lanelet::projection::TransverseMercatorProjector>(projector);
+  }
+  const std::string error_msg =
+    "Invalid map projector type: " + projector_info.projector_type +
+    ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator";
+  throw std::invalid_argument(error_msg);
+}
+
+}  // namespace autoware::geography_utils

common/autoware_geography_utils/src/projection.cpp

@@ -0,0 +1,95 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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 <GeographicLib/Geoid.hpp>
+#include <autoware/geography_utils/lanelet2_projector.hpp>
+#include <autoware/geography_utils/projection.hpp>
+#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
+
+namespace autoware::geography_utils
+{
+
+Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src)
+{
+  Eigen::Vector3d dst;
+  dst.x() = src.x;
+  dst.y() = src.y;
+  dst.z() = src.z;
+  return dst;
+}
+
+LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info)
+{
+  std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
+  lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude};
+
+  lanelet::BasicPoint3d projected_local_point;
+  if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    const int mgrs_precision = 9;  // set precision as 100 micro meter
+    const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
+
+    // project x and y using projector
+    // note that the altitude is ignored in MGRS projection conventionally
+    projected_local_point = mgrs_projector->forward(position, mgrs_precision);
+  } else {
+    // project x and y using projector
+    // note that the original projector such as UTM projector does not compensate for the altitude
+    // offset
+    projected_local_point = projector->forward(position);
+
+    // correct z based on the map origin
+    // note that the converted altitude in local point is in the same vertical datum as the geo
+    // point
+    projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude;
+  }
+
+  LocalPoint local_point;
+  local_point.x = projected_local_point.x();
+  local_point.y = projected_local_point.y();
+  local_point.z = projected_local_point.z();
+
+  return local_point;
+}
+
+GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info)
+{
+  std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
+
+  lanelet::GPSPoint projected_gps_point;
+  if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
+    // project latitude and longitude using projector
+    // note that the z is ignored in MGRS projection conventionally
+    projected_gps_point =
+      mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid);
+  } else {
+    // project latitude and longitude using projector
+    // note that the original projector such as UTM projector does not compensate for the altitude
+    // offset
+    projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point));
+
+    // correct altitude based on the map origin
+    // note that the converted altitude in local point is in the same vertical datum as the geo
+    // point
+    projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude;
+  }
+
+  GeoPoint geo_point;
+  geo_point.latitude = projected_gps_point.lat;
+  geo_point.longitude = projected_gps_point.lon;
+  geo_point.altitude = projected_gps_point.ele;
+  return geo_point;
+}
+
+}  // namespace autoware::geography_utils