feat(geography_utils): add geography_utils package

common/geography_utils/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.14)
+project(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(geography_utils SHARED
+  src/height.cpp
+)
+
+target_link_libraries(geography_utils
+  ${GeographicLib_LIBRARIES}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_geography_utils ${test_files})
+
+  target_link_libraries(test_geography_utils
+  geography_utils
+  )
+endif()
+
+ament_auto_package()

common/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/geography_utils/include/geography_utils/height.hpp

@@ -0,0 +1,36 @@
+// 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 GEOGRAPHY_UTILS__HEIGHT_HPP_
+#define GEOGRAPHY_UTILS__HEIGHT_HPP_
+
+#include <map>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+namespace 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 geography_utils
+
+#endif  // GEOGRAPHY_UTILS__HEIGHT_HPP_

common/geography_utils/package.xml

@@ -0,0 +1,22 @@
+<?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>geography_utils</name>
+  <version>0.1.0</version>
+  <description>The geography_utils package</description>
+  <maintainer email="[email protected]">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>geographiclib</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/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 <geography_utils/height.hpp>
+
+#include <map>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+namespace 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 geography_utils

common/geography_utils/test/test_height.cpp

@@ -0,0 +1,92 @@
+// 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 <geography_utils/height.hpp>
+
+#include <gtest/gtest.h>
+
+#include <stdexcept>
+#include <string>
+
+// Test case to verify if same source and target datums return original height
+TEST(Tier4GeographyUtils, SameSourceTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+  const std::string datum = "WGS84";
+
+  double converted_height =
+    geography_utils::convert_height(height, latitude, longitude, datum, datum);
+
+  EXPECT_DOUBLE_EQ(height, converted_height);
+}
+
+// Test case to verify valid source and target datums
+TEST(Tier4GeographyUtils, ValidSourceTargetDatum)
+{
+  // Calculated with
+  // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+  const double target_height = -30.18;
+
+  double converted_height =
+    geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008");
+
+  EXPECT_NEAR(target_height, converted_height, 0.1);
+}
+
+// Test case to verify invalid source and target datums
+TEST(Tier4GeographyUtils, InvalidSourceTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"),
+    std::invalid_argument);
+}
+
+// Test case to verify invalid source datums
+TEST(Tier4GeographyUtils, InvalidSourceDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"),
+    std::invalid_argument);
+}
+
+// Test case to verify invalid target datums
+TEST(Tier4GeographyUtils, InvalidTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"),
+    std::invalid_argument);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

sensing/gnss_poser/include/gnss_poser/convert.hpp

@@ -19,6 +19,7 @@
 #include <GeographicLib/Geoid.hpp>
 #include <GeographicLib/MGRS.hpp>
 #include <GeographicLib/UTMUPS.hpp>
+#include <geography_utils/height.hpp>
 #include <rclcpp/logging.hpp>
 
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
@@ -38,23 +39,7 @@
   _1_MIllI_METER = 8,
   _100MICRO_METER = 9,
 };
-// EllipsoidHeight:height above ellipsoid
-// OrthometricHeight:height above geoid
-double EllipsoidHeight2OrthometricHeight(
-  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger)
-{
-  double OrthometricHeight{0.0};
-  try {
-    GeographicLib::Geoid egm2008("egm2008-1");
-    OrthometricHeight = egm2008.ConvertHeight(
-      nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude,
-      GeographicLib::Geoid::ELLIPSOIDTOGEOID);
-  } catch (const GeographicLib::GeographicErr & err) {
-    RCLCPP_ERROR_STREAM(
-      logger, "Failed to convert Height from Ellipsoid to Orthometric" << err.what());
-  }
-  return OrthometricHeight;
-}
+
 GNSSStat NavSatFix2UTM(
   const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger,
   int height_system)
@@ -65,11 +50,16 @@
     GeographicLib::UTMUPS::Forward(
       nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x,
       utm.y);
+
+    std::string target_height_system;
     if (height_system == 0) {
-      utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger);
+      target_height_system = "EGM2008";
     } else {
-      utm.z = nav_sat_fix_msg.altitude;
+      target_height_system = "WGS84";
     }
+    utm.z = geography_utils::convert_height(
+      nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84",
+      target_height_system);
     utm.latitude = nav_sat_fix_msg.latitude;
     utm.longitude = nav_sat_fix_msg.longitude;
     utm.altitude = nav_sat_fix_msg.altitude;
@@ -78,6 +68,7 @@
   }
   return utm;
 }
+
 GNSSStat NavSatFix2LocalCartesianUTM(
   const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
   sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system)
@@ -89,11 +80,15 @@
     GeographicLib::UTMUPS::Forward(
       nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone,
       utm_origin.east_north_up, utm_origin.x, utm_origin.y);
+    std::string target_height_system;
     if (height_system == 0) {
-      utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger);
+      target_height_system = "EGM2008";
     } else {
-      utm_origin.z = nav_sat_fix_origin.altitude;
+      target_height_system = "WGS84";
     }
+    utm_origin.z = geography_utils::convert_height(
+      nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude,
+      "WGS84", target_height_system);
 
     // individual coordinates of global coordinate system
     double global_x = 0.0;
@@ -107,17 +102,17 @@
     // individual coordinates of local coordinate system
     utm_local.x = global_x - utm_origin.x;
     utm_local.y = global_y - utm_origin.y;
-    if (height_system == 0) {
-      utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z;
-    } else {
-      utm_local.z = nav_sat_fix_msg.altitude - utm_origin.z;
-    }
+    utm_local.z = geography_utils::convert_height(
+                    nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude,
+                    "WGS84", target_height_system) -
+                  utm_origin.z;
   } catch (const GeographicLib::GeographicErr & err) {
     RCLCPP_ERROR_STREAM(
       logger, "Failed to convert from LLH to UTM in local coordinates" << err.what());
   }
   return utm_local;
 }
+
 GNSSStat UTM2MGRS(
   const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger)
 {

sensing/gnss_poser/package.xml

@@ -15,6 +15,7 @@
 
   <depend>autoware_sensing_msgs</depend>
   <depend>geographiclib</depend>
+  <depend>geography_utils</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

system/default_ad_api/package.xml

@@ -23,6 +23,7 @@
   <depend>autoware_planning_msgs</depend>
   <depend>component_interface_specs</depend>
   <depend>component_interface_utils</depend>
+  <depend>geography_utils</depend>
   <depend>lanelet2_extension</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>