From 10cbe930abc5388dc0278f4d850590dde892b680 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 09:30:59 +0900 Subject: [PATCH 01/28] first commit Signed-off-by: kminoda --- .../tier4_autoware_utils/geography/height.hpp | 57 +++++++++++++++++++ .../tier4_autoware_utils.hpp | 1 + .../gnss_poser/include/gnss_poser/convert.hpp | 38 ++++--------- sensing/gnss_poser/package.xml | 1 + system/default_ad_api/src/vehicle.cpp | 9 ++- 5 files changed, 78 insertions(+), 28 deletions(-) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp new file mode 100644 index 0000000000000..546c3fc7593b0 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp @@ -0,0 +1,57 @@ +// 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 TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ +#define TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +typedef double (*HeightConversionFunction)(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude); + +double convert_wgs84_to_egm2008(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) +{ + return height; +} + +double convert_egm2008_to_wgs84(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) +{ + return height; +} + +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, 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 { + throw std::invalid_argument("Invalid conversion types"); + } +} + +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index 2aa5ae0c7e6ef..ad81a7ec97e74 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -15,6 +15,7 @@ #ifndef TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ #define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ +#include "tier4_autoware_utils/geography/height.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 74c9734833232..0bba32585ef6f 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -22,6 +22,7 @@ #include #include +#include #include @@ -38,23 +39,7 @@ enum class MGRSPrecision { _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,14 @@ GNSSStat NavSatFix2UTM( 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 = tier4_autoware_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; @@ -89,11 +77,13 @@ GNSSStat NavSatFix2LocalCartesianUTM( 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 = tier4_autoware_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,11 +97,7 @@ GNSSStat NavSatFix2LocalCartesianUTM( // 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 = tier4_autoware_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()); diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 1488f329edecb..32b46539929d5 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index fb434adbb0e40..2e59cbfe8a252 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "vehicle.hpp" +#include "tier4_autoware_utils/geography/height.hpp" #include @@ -153,7 +154,9 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.altitude = tier4_autoware_utils::convert_height( + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, + "WGS84"); } else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { lanelet::GPSPoint position{ map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; @@ -165,7 +168,9 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.altitude = tier4_autoware_utils::convert_height( + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, + "WGS84"); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = From 765da177d10af220ff62a323f0696ab215a58ea6 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 11:03:05 +0900 Subject: [PATCH 02/28] update Signed-off-by: kminoda --- .../tier4_autoware_utils/geography/height.hpp | 32 ++--------- .../src/geography/height.cpp | 53 +++++++++++++++++++ .../test/src/geography/test_height.cpp | 20 +++++++ 3 files changed, 77 insertions(+), 28 deletions(-) create mode 100644 common/tier4_autoware_utils/src/geography/height.cpp create mode 100644 common/tier4_autoware_utils/test/src/geography/test_height.cpp diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp index 546c3fc7593b0..646c6a005b0df 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__GROGRAPHY__HEIGHT_HPP_ #define TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ #include @@ -24,33 +24,9 @@ namespace tier4_autoware_utils { typedef double (*HeightConversionFunction)(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude); - -double convert_wgs84_to_egm2008(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) -{ - return height; -} - -double convert_egm2008_to_wgs84(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) -{ - return height; -} - -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, 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 { - throw std::invalid_argument("Invalid conversion types"); - } -} +double convert_wgs84_to_egm2008(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude); +double convert_egm2008_to_wgs84(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] 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 tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/geography/height.cpp b/common/tier4_autoware_utils/src/geography/height.cpp new file mode 100644 index 0000000000000..dedd002e21364 --- /dev/null +++ b/common/tier4_autoware_utils/src/geography/height.cpp @@ -0,0 +1,53 @@ +// 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 "tier4_autoware_utils/geography/height.hpp" +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +double convert_wgs84_to_egm2008(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) +{ + return height; +} + +double convert_egm2008_to_wgs84(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) +{ + return height; +} + +double convert_height(const double height, const double latitude, const double longitude, const std::string & source_vertical_datum, const std::string & target_vertical_datum) +{ + // (void)latitude; (void)longitude; (void)source_vertical_datum; (void)target_vertical_datum; (void)height; + // return 0; + if (source_vertical_datum == target_vertical_datum) { + return height; + } + std::map, 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 { + throw std::invalid_argument("Invalid conversion types"); + } +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geography/test_height.cpp b/common/tier4_autoware_utils/test/src/geography/test_height.cpp new file mode 100644 index 0000000000000..a4b64da64975f --- /dev/null +++ b/common/tier4_autoware_utils/test/src/geography/test_height.cpp @@ -0,0 +1,20 @@ +// Copyright 2020 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 "tier4_autoware_utils/geography/height.hpp" +#include + +#include + +// TODO \ No newline at end of file From c09732d60d598ff202b8411c96882e0c37adc5a0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 02:05:08 +0000 Subject: [PATCH 03/28] style(pre-commit): autofix --- .../tier4_autoware_utils/geography/height.hpp | 22 +++++++++++++------ .../src/geography/height.cpp | 21 ++++++++++++------ .../test/src/geography/test_height.cpp | 3 ++- .../gnss_poser/include/gnss_poser/convert.hpp | 17 +++++++++----- sensing/gnss_poser/package.xml | 2 +- system/default_ad_api/src/vehicle.cpp | 9 ++++---- 6 files changed, 49 insertions(+), 25 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp index 646c6a005b0df..f876771e47d92 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp @@ -12,21 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GROGRAPHY__HEIGHT_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ #define TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ -#include #include -#include #include +#include +#include namespace tier4_autoware_utils { -typedef double (*HeightConversionFunction)(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude); -double convert_wgs84_to_egm2008(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude); -double convert_egm2008_to_wgs84(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] 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); +typedef double (*HeightConversionFunction)( + const double height, [[maybe_unused]] const double latitude, + [[maybe_unused]] const double longitude); +double convert_wgs84_to_egm2008( + const double height, [[maybe_unused]] const double latitude, + [[maybe_unused]] const double longitude); +double convert_egm2008_to_wgs84( + const double height, [[maybe_unused]] const double latitude, + [[maybe_unused]] 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 tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/geography/height.cpp b/common/tier4_autoware_utils/src/geography/height.cpp index dedd002e21364..ef2ee0725e605 100644 --- a/common/tier4_autoware_utils/src/geography/height.cpp +++ b/common/tier4_autoware_utils/src/geography/height.cpp @@ -13,28 +13,35 @@ // limitations under the License. #include "tier4_autoware_utils/geography/height.hpp" -#include + #include -#include #include +#include +#include namespace tier4_autoware_utils { -double convert_wgs84_to_egm2008(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) +double convert_wgs84_to_egm2008( + const double height, [[maybe_unused]] const double latitude, + [[maybe_unused]] const double longitude) { return height; } -double convert_egm2008_to_wgs84(const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) +double convert_egm2008_to_wgs84( + const double height, [[maybe_unused]] const double latitude, + [[maybe_unused]] const double longitude) { return height; } -double convert_height(const double height, const double latitude, const double longitude, const std::string & source_vertical_datum, const std::string & target_vertical_datum) +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum) { - // (void)latitude; (void)longitude; (void)source_vertical_datum; (void)target_vertical_datum; (void)height; - // return 0; + // (void)latitude; (void)longitude; (void)source_vertical_datum; (void)target_vertical_datum; + // (void)height; return 0; if (source_vertical_datum == target_vertical_datum) { return height; } diff --git a/common/tier4_autoware_utils/test/src/geography/test_height.cpp b/common/tier4_autoware_utils/test/src/geography/test_height.cpp index a4b64da64975f..ede491ffe9b21 100644 --- a/common/tier4_autoware_utils/test/src/geography/test_height.cpp +++ b/common/tier4_autoware_utils/test/src/geography/test_height.cpp @@ -13,8 +13,9 @@ // limitations under the License. #include "tier4_autoware_utils/geography/height.hpp" + #include #include -// TODO \ No newline at end of file +// TODO diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 0bba32585ef6f..fcaa34f709627 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -20,9 +20,9 @@ #include #include #include +#include #include -#include #include @@ -50,14 +50,16 @@ GNSSStat NavSatFix2UTM( 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) { target_height_system = "EGM2008"; } else { target_height_system = "WGS84"; } - utm.z = tier4_autoware_utils::convert_height(nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", target_height_system); + utm.z = tier4_autoware_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; @@ -83,7 +85,9 @@ GNSSStat NavSatFix2LocalCartesianUTM( } else { target_height_system = "WGS84"; } - utm_origin.z = tier4_autoware_utils::convert_height(nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, "WGS84", target_height_system); + utm_origin.z = tier4_autoware_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; @@ -97,7 +101,10 @@ GNSSStat NavSatFix2LocalCartesianUTM( // individual coordinates of local coordinate system utm_local.x = global_x - utm_origin.x; utm_local.y = global_y - utm_origin.y; - utm_local.z = tier4_autoware_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; + utm_local.z = tier4_autoware_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()); diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 32b46539929d5..9cea7e0e64cbd 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -22,8 +22,8 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_debug_msgs tier4_autoware_utils + tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 2e59cbfe8a252..183ccf258c32b 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "vehicle.hpp" + #include "tier4_autoware_utils/geography/height.hpp" #include @@ -155,8 +156,8 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; vehicle_kinematics.geographic_pose.position.altitude = tier4_autoware_utils::convert_height( - projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, - "WGS84"); + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, + map_projector_info_->vertical_datum, "WGS84"); } else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { lanelet::GPSPoint position{ map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; @@ -169,8 +170,8 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; vehicle_kinematics.geographic_pose.position.altitude = tier4_autoware_utils::convert_height( - projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, - "WGS84"); + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, + map_projector_info_->vertical_datum, "WGS84"); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = From 149bfe7fff511c9da9a3cb285826955730697f1f Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 11:16:55 +0900 Subject: [PATCH 04/28] update height.cpp Signed-off-by: kminoda --- common/tier4_autoware_utils/package.xml | 1 + .../tier4_autoware_utils/src/geography/height.cpp | 15 +++++++++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index fd9b315f8e4d5..c6ff4ee89240f 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -27,6 +27,7 @@ tier4_debug_msgs unique_identifier_msgs visualization_msgs + geographiclib ament_cmake_ros ament_lint_auto diff --git a/common/tier4_autoware_utils/src/geography/height.cpp b/common/tier4_autoware_utils/src/geography/height.cpp index ef2ee0725e605..eaf0e6199135d 100644 --- a/common/tier4_autoware_utils/src/geography/height.cpp +++ b/common/tier4_autoware_utils/src/geography/height.cpp @@ -14,6 +14,9 @@ #include "tier4_autoware_utils/geography/height.hpp" +#include + +#include #include #include #include @@ -26,22 +29,26 @@ double convert_wgs84_to_egm2008( const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) { - return height; + double converted_height{0.0}; + GeographicLib::Geoid egm2008("egm2008-1"); + converted_height = egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); + return converted_height; } double convert_egm2008_to_wgs84( const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) { - return height; + double converted_height{0.0}; + GeographicLib::Geoid egm2008("egm2008-1"); + converted_height = egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); + return converted_height; } double convert_height( const double height, const double latitude, const double longitude, const std::string & source_vertical_datum, const std::string & target_vertical_datum) { - // (void)latitude; (void)longitude; (void)source_vertical_datum; (void)target_vertical_datum; - // (void)height; return 0; if (source_vertical_datum == target_vertical_datum) { return height; } From 5d087fb72cfcb0631c27fd40df82ac25e8c0a25d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 02:20:17 +0000 Subject: [PATCH 05/28] style(pre-commit): autofix --- common/tier4_autoware_utils/package.xml | 2 +- common/tier4_autoware_utils/src/geography/height.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index c6ff4ee89240f..3152206a8837f 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -17,6 +17,7 @@ autoware_auto_vehicle_msgs builtin_interfaces diagnostic_msgs + geographiclib geometry_msgs libboost-dev pcl_conversions @@ -27,7 +28,6 @@ tier4_debug_msgs unique_identifier_msgs visualization_msgs - geographiclib ament_cmake_ros ament_lint_auto diff --git a/common/tier4_autoware_utils/src/geography/height.cpp b/common/tier4_autoware_utils/src/geography/height.cpp index eaf0e6199135d..23dcbd75e2b28 100644 --- a/common/tier4_autoware_utils/src/geography/height.cpp +++ b/common/tier4_autoware_utils/src/geography/height.cpp @@ -16,7 +16,6 @@ #include -#include #include #include #include @@ -31,7 +30,8 @@ double convert_wgs84_to_egm2008( { double converted_height{0.0}; GeographicLib::Geoid egm2008("egm2008-1"); - converted_height = egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); + converted_height = + egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); return converted_height; } @@ -41,7 +41,8 @@ double convert_egm2008_to_wgs84( { double converted_height{0.0}; GeographicLib::Geoid egm2008("egm2008-1"); - converted_height = egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); + converted_height = + egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); return converted_height; } From 118bf5659de66df8cf1fb5e3b27120b5aa8fa525 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 14:57:20 +0900 Subject: [PATCH 06/28] updat Signed-off-by: kminoda --- .../tier4_autoware_utils.hpp | 1 - common/tier4_geography_utils/CMakeLists.txt | 37 +++++ .../include/tier4_geography_utils}/height.hpp | 10 +- common/tier4_geography_utils/package.xml | 22 +++ .../src}/height.cpp | 23 ++- .../test}/test_height.cpp | 2 +- sensing/gnss_poser/CMakeLists.txt | 1 + .../gnss_poser/include/gnss_poser/convert.hpp | 107 +------------ sensing/gnss_poser/package.xml | 2 +- sensing/gnss_poser/src/convert.cpp | 140 ++++++++++++++++++ system/default_ad_api/package.xml | 1 + system/default_ad_api/src/vehicle.cpp | 6 +- 12 files changed, 227 insertions(+), 125 deletions(-) create mode 100644 common/tier4_geography_utils/CMakeLists.txt rename common/{tier4_autoware_utils/include/tier4_autoware_utils/geography => tier4_geography_utils/include/tier4_geography_utils}/height.hpp (84%) create mode 100644 common/tier4_geography_utils/package.xml rename common/{tier4_autoware_utils/src/geography => tier4_geography_utils/src}/height.cpp (74%) rename common/{tier4_autoware_utils/test/src/geography => tier4_geography_utils/test}/test_height.cpp (92%) create mode 100644 sensing/gnss_poser/src/convert.cpp diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index ad81a7ec97e74..2aa5ae0c7e6ef 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -15,7 +15,6 @@ #ifndef TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ #define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ -#include "tier4_autoware_utils/geography/height.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/common/tier4_geography_utils/CMakeLists.txt b/common/tier4_geography_utils/CMakeLists.txt new file mode 100644 index 0000000000000..d8de2bba614f2 --- /dev/null +++ b/common/tier4_geography_utils/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_geography_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED) + +# 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(tier4_geography_utils SHARED + src/height.cpp +) + +target_link_libraries(tier4_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_tier4_geography_utils ${test_files}) + +# target_link_libraries(test_tier4_geography_utils +# tier4_geography_utils +# ) +# endif() + +ament_auto_package() diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp similarity index 84% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp rename to common/tier4_geography_utils/include/tier4_geography_utils/height.hpp index f876771e47d92..4920767331284 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geography/height.hpp +++ b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ +#ifndef TIER4_GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define TIER4_GEOGRAPHY_UTILS__HEIGHT_HPP_ #include #include #include #include -namespace tier4_autoware_utils +namespace tier4_geography_utils { typedef double (*HeightConversionFunction)( @@ -36,6 +36,6 @@ 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 tier4_autoware_utils +} // namespace tier4_geography_utils -#endif // TIER4_AUTOWARE_UTILS__GEOGRAPHY__HEIGHT_HPP_ +#endif // TIER4_GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/tier4_geography_utils/package.xml b/common/tier4_geography_utils/package.xml new file mode 100644 index 0000000000000..821b1c63eff28 --- /dev/null +++ b/common/tier4_geography_utils/package.xml @@ -0,0 +1,22 @@ + + + + tier4_geography_utils + 0.1.0 + The tier4_geography_utils package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geographiclib + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/tier4_autoware_utils/src/geography/height.cpp b/common/tier4_geography_utils/src/height.cpp similarity index 74% rename from common/tier4_autoware_utils/src/geography/height.cpp rename to common/tier4_geography_utils/src/height.cpp index 23dcbd75e2b28..b6e7e07257e95 100644 --- a/common/tier4_autoware_utils/src/geography/height.cpp +++ b/common/tier4_geography_utils/src/height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geography/height.hpp" +#include "tier4_geography_utils/height.hpp" #include @@ -21,29 +21,23 @@ #include #include -namespace tier4_autoware_utils +namespace tier4_geography_utils { double convert_wgs84_to_egm2008( const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) { - double converted_height{0.0}; GeographicLib::Geoid egm2008("egm2008-1"); - converted_height = - egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); - return converted_height; + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); } double convert_egm2008_to_wgs84( const double height, [[maybe_unused]] const double latitude, [[maybe_unused]] const double longitude) { - double converted_height{0.0}; GeographicLib::Geoid egm2008("egm2008-1"); - converted_height = - egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); - return converted_height; + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); } double convert_height( @@ -61,8 +55,13 @@ double convert_height( if (conversion_map.find(key) != conversion_map.end()) { return conversion_map[key](height, latitude, longitude); } else { - throw std::invalid_argument("Invalid conversion types"); + 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 tier4_autoware_utils +} // namespace tier4_geography_utils diff --git a/common/tier4_autoware_utils/test/src/geography/test_height.cpp b/common/tier4_geography_utils/test/test_height.cpp similarity index 92% rename from common/tier4_autoware_utils/test/src/geography/test_height.cpp rename to common/tier4_geography_utils/test/test_height.cpp index ede491ffe9b21..d484cfb578d0e 100644 --- a/common/tier4_autoware_utils/test/src/geography/test_height.cpp +++ b/common/tier4_geography_utils/test/test_height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geography/height.hpp" +#include "tier4_geography_utils/geography/height.hpp" #include diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 993cf2d9da9e1..beed926f0c8fa 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -23,6 +23,7 @@ set(GNSS_POSER_HEADERS ament_auto_add_library(gnss_poser_node SHARED src/gnss_poser_core.cpp + src/convert.cpp ${GNSS_POSER_HEADERS} ) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index fcaa34f709627..60901abb11915 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -16,11 +16,8 @@ #include "gnss_poser/gnss_stat.hpp" -#include -#include -#include #include -#include +#include "tier4_geography_utils/height.hpp" #include @@ -42,112 +39,18 @@ enum class MGRSPrecision { GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - int height_system) -{ - GNSSStat utm; - - try { - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, - utm.y); + int height_system); - std::string target_height_system; - if (height_system == 0) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } - utm.z = tier4_autoware_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; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); - } - 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) -{ - GNSSStat utm_local; - try { - // origin of the local coordinate system in global frame - GNSSStat utm_origin; - 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) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } - utm_origin.z = tier4_autoware_utils::convert_height( - nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, - "WGS84", target_height_system); + sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system); - // individual coordinates of global coordinate system - double global_x = 0.0; - double global_y = 0.0; - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, - utm_origin.east_north_up, global_x, global_y); - utm_local.latitude = nav_sat_fix_msg.latitude; - utm_local.longitude = nav_sat_fix_msg.longitude; - utm_local.altitude = nav_sat_fix_msg.altitude; - // individual coordinates of local coordinate system - utm_local.x = global_x - utm_origin.x; - utm_local.y = global_y - utm_origin.y; - utm_local.z = tier4_autoware_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) -{ - constexpr int GZD_ID_size = 5; // size of header like "53SPU" - - GNSSStat mgrs = utm; - try { - std::string mgrs_code; - GeographicLib::MGRS::Forward( - utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), - mgrs_code); - mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); - mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.y = std::stod(mgrs_code.substr( - GZD_ID_size + static_cast(precision), static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.z = utm.z; // TODO(ryo.watanabe) - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); - } - return mgrs; -} + const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger); GNSSStat NavSatFix2MGRS( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, int height_system) -{ - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); - const auto mgrs = UTM2MGRS(utm, precision, logger); - return mgrs; -} - + const rclcpp::Logger & logger, int height_system); } // namespace gnss_poser #endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 9cea7e0e64cbd..5a0639e66338f 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -22,7 +22,7 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils + tier4_geography_utils tier4_debug_msgs ament_lint_auto diff --git a/sensing/gnss_poser/src/convert.cpp b/sensing/gnss_poser/src/convert.cpp new file mode 100644 index 0000000000000..d1812cf042149 --- /dev/null +++ b/sensing/gnss_poser/src/convert.cpp @@ -0,0 +1,140 @@ +// Copyright 2020 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 "gnss_poser/gnss_stat.hpp" +#include "gnss_poser/convert.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +namespace gnss_poser +{ +GNSSStat NavSatFix2UTM( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, + int height_system) +{ + GNSSStat utm; + + try { + 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) { + target_height_system = "EGM2008"; + } else { + target_height_system = "WGS84"; + } + utm.z = tier4_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; + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); + } + 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) +{ + GNSSStat utm_local; + try { + // origin of the local coordinate system in global frame + GNSSStat utm_origin; + 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) { + target_height_system = "EGM2008"; + } else { + target_height_system = "WGS84"; + } + utm_origin.z = tier4_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; + double global_y = 0.0; + GeographicLib::UTMUPS::Forward( + nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, + utm_origin.east_north_up, global_x, global_y); + utm_local.latitude = nav_sat_fix_msg.latitude; + utm_local.longitude = nav_sat_fix_msg.longitude; + utm_local.altitude = nav_sat_fix_msg.altitude; + // individual coordinates of local coordinate system + utm_local.x = global_x - utm_origin.x; + utm_local.y = global_y - utm_origin.y; + utm_local.z = tier4_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) +{ + constexpr int GZD_ID_size = 5; // size of header like "53SPU" + + GNSSStat mgrs = utm; + try { + std::string mgrs_code; + GeographicLib::MGRS::Forward( + utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), + mgrs_code); + mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); + mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * + std::pow( + 10, static_cast(MGRSPrecision::_1_METER) - + static_cast(precision)); // set unit as [m] + mgrs.y = std::stod(mgrs_code.substr( + GZD_ID_size + static_cast(precision), static_cast(precision))) * + std::pow( + 10, static_cast(MGRSPrecision::_1_METER) - + static_cast(precision)); // set unit as [m] + mgrs.z = utm.z; // TODO(ryo.watanabe) + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); + } + return mgrs; +} + +GNSSStat NavSatFix2MGRS( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, + const rclcpp::Logger & logger, int height_system) +{ + const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); + const auto mgrs = UTM2MGRS(utm, precision, logger); + return mgrs; +} + +} // namespace gnss_poser diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index dfceeabe41122..f2fb0eb79d082 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -34,6 +34,7 @@ tier4_control_msgs tier4_system_msgs vehicle_info_util + tier4_geography_utils python3-flask diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 183ccf258c32b..ad228a05f2c9d 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,7 +14,7 @@ #include "vehicle.hpp" -#include "tier4_autoware_utils/geography/height.hpp" +#include "tier4_geography_utils/height.hpp" #include @@ -155,7 +155,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = tier4_autoware_utils::convert_height( + vehicle_kinematics.geographic_pose.position.altitude = tier4_geography_utils::convert_height( projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, "WGS84"); } else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { @@ -169,7 +169,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = tier4_autoware_utils::convert_height( + vehicle_kinematics.geographic_pose.position.altitude = tier4_geography_utils::convert_height( projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, "WGS84"); } else { From b64192c63e797e0153f4e08f0b58f21f0f72249f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 05:59:32 +0000 Subject: [PATCH 07/28] style(pre-commit): autofix --- common/tier4_geography_utils/src/height.cpp | 7 +++---- sensing/gnss_poser/include/gnss_poser/convert.hpp | 2 +- sensing/gnss_poser/package.xml | 2 +- sensing/gnss_poser/src/convert.cpp | 3 ++- system/default_ad_api/package.xml | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/common/tier4_geography_utils/src/height.cpp b/common/tier4_geography_utils/src/height.cpp index b6e7e07257e95..f47eba03d68f4 100644 --- a/common/tier4_geography_utils/src/height.cpp +++ b/common/tier4_geography_utils/src/height.cpp @@ -55,10 +55,9 @@ double convert_height( 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()); + 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); } diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 60901abb11915..87f44ee7483dc 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -15,9 +15,9 @@ #define GNSS_POSER__CONVERT_HPP_ #include "gnss_poser/gnss_stat.hpp" +#include "tier4_geography_utils/height.hpp" #include -#include "tier4_geography_utils/height.hpp" #include diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 5a0639e66338f..8544688343e98 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -22,8 +22,8 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_geography_utils tier4_debug_msgs + tier4_geography_utils ament_lint_auto autoware_lint_common diff --git a/sensing/gnss_poser/src/convert.cpp b/sensing/gnss_poser/src/convert.cpp index d1812cf042149..6d2827cb872a4 100644 --- a/sensing/gnss_poser/src/convert.cpp +++ b/sensing/gnss_poser/src/convert.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gnss_poser/gnss_stat.hpp" #include "gnss_poser/convert.hpp" +#include "gnss_poser/gnss_stat.hpp" + #include #include #include diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index f2fb0eb79d082..119b90be05b65 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -32,9 +32,9 @@ std_srvs tier4_api_msgs tier4_control_msgs + tier4_geography_utils tier4_system_msgs vehicle_info_util - tier4_geography_utils python3-flask From fd503610cad5f7bdf4ce53e88de68352fae4aa22 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:03:33 +0900 Subject: [PATCH 08/28] add test Signed-off-by: kminoda --- common/tier4_geography_utils/CMakeLists.txt | 16 +++--- common/tier4_geography_utils/README.md | 5 ++ .../include/tier4_geography_utils/height.hpp | 2 +- common/tier4_geography_utils/src/height.cpp | 2 +- .../test/test_height.cpp | 51 +++++++++++++++++-- 5 files changed, 63 insertions(+), 13 deletions(-) create mode 100644 common/tier4_geography_utils/README.md diff --git a/common/tier4_geography_utils/CMakeLists.txt b/common/tier4_geography_utils/CMakeLists.txt index d8de2bba614f2..065c6c5e544f9 100644 --- a/common/tier4_geography_utils/CMakeLists.txt +++ b/common/tier4_geography_utils/CMakeLists.txt @@ -22,16 +22,16 @@ target_link_libraries(tier4_geography_utils ${GeographicLib_LIBRARIES} ) -# if(BUILD_TESTING) -# find_package(ament_cmake_ros REQUIRED) +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) -# file(GLOB_RECURSE test_files test/**/*.cpp) + file(GLOB_RECURSE test_files test/*.cpp) -# ament_add_ros_isolated_gtest(test_tier4_geography_utils ${test_files}) + ament_add_ros_isolated_gtest(test_tier4_geography_utils ${test_files}) -# target_link_libraries(test_tier4_geography_utils -# tier4_geography_utils -# ) -# endif() + target_link_libraries(test_tier4_geography_utils + tier4_geography_utils + ) +endif() ament_auto_package() diff --git a/common/tier4_geography_utils/README.md b/common/tier4_geography_utils/README.md new file mode 100644 index 0000000000000..0c28cc7f9ad18 --- /dev/null +++ b/common/tier4_geography_utils/README.md @@ -0,0 +1,5 @@ +# tier4_geography_utils + +## Purpose + +This package contains geography-related functions used by other packages, so please refer to them as needed. diff --git a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp index 4920767331284..fcc43663fbbd2 100644 --- a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp +++ b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. diff --git a/common/tier4_geography_utils/src/height.cpp b/common/tier4_geography_utils/src/height.cpp index f47eba03d68f4..ebebafce78c94 100644 --- a/common/tier4_geography_utils/src/height.cpp +++ b/common/tier4_geography_utils/src/height.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. diff --git a/common/tier4_geography_utils/test/test_height.cpp b/common/tier4_geography_utils/test/test_height.cpp index d484cfb578d0e..bb9f34e8b849f 100644 --- a/common/tier4_geography_utils/test/test_height.cpp +++ b/common/tier4_geography_utils/test/test_height.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -12,10 +12,55 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_geography_utils/geography/height.hpp" +#include "tier4_geography_utils/height.hpp" #include +#include #include -// TODO +// 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 = tier4_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) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + double converted_height = tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + + // As this is a conversion, just verify that it does not return the original height + // For precise testing, you'll need the exact expected values + EXPECT_NE(height, converted_height); +} + +// 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( + tier4_geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + std::invalid_argument + ); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From 247f2a3e7556ddd3ef1b0a4ad79fcd1ea61f99cb Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:04:16 +0900 Subject: [PATCH 09/28] revert tier4_autoware_utils Signed-off-by: kminoda --- common/tier4_autoware_utils/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 3152206a8837f..fd9b315f8e4d5 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -17,7 +17,6 @@ autoware_auto_vehicle_msgs builtin_interfaces diagnostic_msgs - geographiclib geometry_msgs libboost-dev pcl_conversions From d4617ca039847de339a9fbefc88f923ff52ac32f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 06:06:58 +0000 Subject: [PATCH 10/28] style(pre-commit): autofix --- .../test/test_height.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/common/tier4_geography_utils/test/test_height.cpp b/common/tier4_geography_utils/test/test_height.cpp index bb9f34e8b849f..9ef15348ada7d 100644 --- a/common/tier4_geography_utils/test/test_height.cpp +++ b/common/tier4_geography_utils/test/test_height.cpp @@ -26,9 +26,10 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum) const double latitude = 35.0; const double longitude = 139.0; const std::string datum = "WGS84"; - - double converted_height = tier4_geography_utils::convert_height(height, latitude, longitude, datum, datum); - + + double converted_height = + tier4_geography_utils::convert_height(height, latitude, longitude, datum, datum); + EXPECT_DOUBLE_EQ(height, converted_height); } @@ -38,9 +39,10 @@ TEST(Tier4GeographyUtils, ValidSourceTargetDatum) const double height = 10.0; const double latitude = 35.0; const double longitude = 139.0; - - double converted_height = tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); - + + double converted_height = + tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + // As this is a conversion, just verify that it does not return the original height // For precise testing, you'll need the exact expected values EXPECT_NE(height, converted_height); @@ -52,15 +54,14 @@ TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) const double height = 10.0; const double latitude = 35.0; const double longitude = 139.0; - + EXPECT_THROW( tier4_geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), - std::invalid_argument - ); + std::invalid_argument); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 140131a1a3b1d52443c14218e6b0a30243eac0fc Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:14:23 +0900 Subject: [PATCH 11/28] fix cspell Signed-off-by: kminoda --- common/tier4_geography_utils/src/height.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/tier4_geography_utils/src/height.cpp b/common/tier4_geography_utils/src/height.cpp index ebebafce78c94..32936787ec21b 100644 --- a/common/tier4_geography_utils/src/height.cpp +++ b/common/tier4_geography_utils/src/height.cpp @@ -29,6 +29,7 @@ double convert_wgs84_to_egm2008( [[maybe_unused]] const double longitude) { GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore ELLIPSOIDTOGEOID return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); } @@ -37,6 +38,7 @@ double convert_egm2008_to_wgs84( [[maybe_unused]] const double longitude) { GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore GEOIDTOELLIPSOID return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); } From 9a89e01bb9c70ab16e705d62f3c782fc552d0079 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:19:21 +0900 Subject: [PATCH 12/28] remove and revert convert.cpp Signed-off-by: kminoda --- sensing/gnss_poser/CMakeLists.txt | 1 - .../gnss_poser/include/gnss_poser/convert.hpp | 109 +++++++++++++- sensing/gnss_poser/src/convert.cpp | 141 ------------------ 3 files changed, 104 insertions(+), 147 deletions(-) delete mode 100644 sensing/gnss_poser/src/convert.cpp diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index beed926f0c8fa..993cf2d9da9e1 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -23,7 +23,6 @@ set(GNSS_POSER_HEADERS ament_auto_add_library(gnss_poser_node SHARED src/gnss_poser_core.cpp - src/convert.cpp ${GNSS_POSER_HEADERS} ) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 87f44ee7483dc..d1220b423da00 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -15,9 +15,12 @@ #define GNSS_POSER__CONVERT_HPP_ #include "gnss_poser/gnss_stat.hpp" -#include "tier4_geography_utils/height.hpp" +#include +#include +#include #include +#include #include @@ -39,18 +42,114 @@ enum class MGRSPrecision { GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - int height_system); + int height_system) +{ + GNSSStat utm; + + try { + 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) { + target_height_system = "EGM2008"; + } else { + target_height_system = "WGS84"; + } + utm.z = tier4_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; + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); + } + 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); + sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) +{ + GNSSStat utm_local; + try { + // origin of the local coordinate system in global frame + GNSSStat utm_origin; + 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) { + target_height_system = "EGM2008"; + } else { + target_height_system = "WGS84"; + } + utm_origin.z = tier4_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; + double global_y = 0.0; + GeographicLib::UTMUPS::Forward( + nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, + utm_origin.east_north_up, global_x, global_y); + utm_local.latitude = nav_sat_fix_msg.latitude; + utm_local.longitude = nav_sat_fix_msg.longitude; + utm_local.altitude = nav_sat_fix_msg.altitude; + // individual coordinates of local coordinate system + utm_local.x = global_x - utm_origin.x; + utm_local.y = global_y - utm_origin.y; + utm_local.z = tier4_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); + const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) +{ + constexpr int GZD_ID_size = 5; // size of header like "53SPU" + + GNSSStat mgrs = utm; + try { + std::string mgrs_code; + GeographicLib::MGRS::Forward( + utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), + mgrs_code); + mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); + mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * + std::pow( + 10, static_cast(MGRSPrecision::_1_METER) - + static_cast(precision)); // set unit as [m] + mgrs.y = std::stod(mgrs_code.substr( + GZD_ID_size + static_cast(precision), static_cast(precision))) * + std::pow( + 10, static_cast(MGRSPrecision::_1_METER) - + static_cast(precision)); // set unit as [m] + mgrs.z = utm.z; // TODO(ryo.watanabe) + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); + } + return mgrs; +} GNSSStat NavSatFix2MGRS( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, int height_system); + const rclcpp::Logger & logger, int height_system) +{ + const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); + const auto mgrs = UTM2MGRS(utm, precision, logger); + return mgrs; +} + } // namespace gnss_poser #endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/src/convert.cpp b/sensing/gnss_poser/src/convert.cpp deleted file mode 100644 index 6d2827cb872a4..0000000000000 --- a/sensing/gnss_poser/src/convert.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// Copyright 2020 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 "gnss_poser/convert.hpp" - -#include "gnss_poser/gnss_stat.hpp" - -#include -#include -#include -#include -#include - -#include - -#include - -namespace gnss_poser -{ -GNSSStat NavSatFix2UTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - int height_system) -{ - GNSSStat utm; - - try { - 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) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } - utm.z = tier4_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; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); - } - 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) -{ - GNSSStat utm_local; - try { - // origin of the local coordinate system in global frame - GNSSStat utm_origin; - 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) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } - utm_origin.z = tier4_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; - double global_y = 0.0; - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, - utm_origin.east_north_up, global_x, global_y); - utm_local.latitude = nav_sat_fix_msg.latitude; - utm_local.longitude = nav_sat_fix_msg.longitude; - utm_local.altitude = nav_sat_fix_msg.altitude; - // individual coordinates of local coordinate system - utm_local.x = global_x - utm_origin.x; - utm_local.y = global_y - utm_origin.y; - utm_local.z = tier4_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) -{ - constexpr int GZD_ID_size = 5; // size of header like "53SPU" - - GNSSStat mgrs = utm; - try { - std::string mgrs_code; - GeographicLib::MGRS::Forward( - utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), - mgrs_code); - mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); - mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.y = std::stod(mgrs_code.substr( - GZD_ID_size + static_cast(precision), static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.z = utm.z; // TODO(ryo.watanabe) - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); - } - return mgrs; -} - -GNSSStat NavSatFix2MGRS( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, int height_system) -{ - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); - const auto mgrs = UTM2MGRS(utm, precision, logger); - return mgrs; -} - -} // namespace gnss_poser From df2b65ee86b8b49a0e622793a875fc597040cf3d Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:22:42 +0900 Subject: [PATCH 13/28] remove boost Signed-off-by: kminoda --- common/tier4_geography_utils/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/common/tier4_geography_utils/CMakeLists.txt b/common/tier4_geography_utils/CMakeLists.txt index 065c6c5e544f9..4d6e021ec78de 100644 --- a/common/tier4_geography_utils/CMakeLists.txt +++ b/common/tier4_geography_utils/CMakeLists.txt @@ -4,8 +4,6 @@ project(tier4_geography_utils) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Boost REQUIRED) - # GeographicLib find_package(PkgConfig) find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h From aa5ee45e0be3156f8767ec586935c9c4b6a0f964 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:31:11 +0900 Subject: [PATCH 14/28] update comment Signed-off-by: kminoda --- common/tier4_geography_utils/test/test_height.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/tier4_geography_utils/test/test_height.cpp b/common/tier4_geography_utils/test/test_height.cpp index 9ef15348ada7d..ba150a22c6c6e 100644 --- a/common/tier4_geography_utils/test/test_height.cpp +++ b/common/tier4_geography_utils/test/test_height.cpp @@ -43,8 +43,8 @@ TEST(Tier4GeographyUtils, ValidSourceTargetDatum) double converted_height = tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + // This is not a precise conversion // As this is a conversion, just verify that it does not return the original height - // For precise testing, you'll need the exact expected values EXPECT_NE(height, converted_height); } From d59675d91f6175c625d4dbaa3ace6c2f195ecf34 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:38:45 +0900 Subject: [PATCH 15/28] remove maybe_unsued Signed-off-by: kminoda --- .../include/tier4_geography_utils/height.hpp | 12 ++++++------ common/tier4_geography_utils/src/height.cpp | 6 ++---- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp index fcc43663fbbd2..ff6ea345ddbf0 100644 --- a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp +++ b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp @@ -24,14 +24,14 @@ namespace tier4_geography_utils { typedef double (*HeightConversionFunction)( - const double height, [[maybe_unused]] const double latitude, - [[maybe_unused]] const double longitude); + const double height, const double latitude, + const double longitude); double convert_wgs84_to_egm2008( - const double height, [[maybe_unused]] const double latitude, - [[maybe_unused]] const double longitude); + const double height, const double latitude, + const double longitude); double convert_egm2008_to_wgs84( - const double height, [[maybe_unused]] const double latitude, - [[maybe_unused]] const double longitude); + 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); diff --git a/common/tier4_geography_utils/src/height.cpp b/common/tier4_geography_utils/src/height.cpp index 32936787ec21b..2198571c12a56 100644 --- a/common/tier4_geography_utils/src/height.cpp +++ b/common/tier4_geography_utils/src/height.cpp @@ -25,8 +25,7 @@ namespace tier4_geography_utils { double convert_wgs84_to_egm2008( - const double height, [[maybe_unused]] const double latitude, - [[maybe_unused]] const double longitude) + const double height, const double latitude, const double longitude) { GeographicLib::Geoid egm2008("egm2008-1"); // cSpell: ignore ELLIPSOIDTOGEOID @@ -34,8 +33,7 @@ double convert_wgs84_to_egm2008( } double convert_egm2008_to_wgs84( - const double height, [[maybe_unused]] const double latitude, - [[maybe_unused]] const double longitude) + const double height, const double latitude, const double longitude) { GeographicLib::Geoid egm2008("egm2008-1"); // cSpell: ignore GEOIDTOELLIPSOID From d0dd3cb08c1bcd937ee667efa3046ff80d0d7302 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 06:40:32 +0000 Subject: [PATCH 16/28] style(pre-commit): autofix --- .../include/tier4_geography_utils/height.hpp | 11 +++-------- common/tier4_geography_utils/src/height.cpp | 6 ++---- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp index ff6ea345ddbf0..0fcb8f757049c 100644 --- a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp +++ b/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp @@ -24,14 +24,9 @@ namespace tier4_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); + 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); diff --git a/common/tier4_geography_utils/src/height.cpp b/common/tier4_geography_utils/src/height.cpp index 2198571c12a56..287b168e11cbd 100644 --- a/common/tier4_geography_utils/src/height.cpp +++ b/common/tier4_geography_utils/src/height.cpp @@ -24,16 +24,14 @@ namespace tier4_geography_utils { -double convert_wgs84_to_egm2008( - const double height, const double latitude, const double longitude) +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) +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) { GeographicLib::Geoid egm2008("egm2008-1"); // cSpell: ignore GEOIDTOELLIPSOID From ba19c122588bcf26165375d0b81a10bff1ff5e4f Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:46:51 +0900 Subject: [PATCH 17/28] rename from tier4_geography_utils to geography_utils Signed-off-by: kminoda --- .../CMakeLists.txt | 12 ++++++------ .../README.md | 2 +- .../include/geography_utils}/height.hpp | 6 +++--- .../package.xml | 4 ++-- .../src/height.cpp | 2 +- .../test/test_height.cpp | 10 +++++----- sensing/gnss_poser/include/gnss_poser/convert.hpp | 2 +- sensing/gnss_poser/package.xml | 2 +- 8 files changed, 20 insertions(+), 20 deletions(-) rename common/{tier4_geography_utils => geography_utils}/CMakeLists.txt (65%) rename common/{tier4_geography_utils => geography_utils}/README.md (83%) rename common/{tier4_geography_utils/include/tier4_geography_utils => geography_utils/include/geography_utils}/height.hpp (90%) rename common/{tier4_geography_utils => geography_utils}/package.xml (86%) rename common/{tier4_geography_utils => geography_utils}/src/height.cpp (98%) rename common/{tier4_geography_utils => geography_utils}/test/test_height.cpp (88%) diff --git a/common/tier4_geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt similarity index 65% rename from common/tier4_geography_utils/CMakeLists.txt rename to common/geography_utils/CMakeLists.txt index 4d6e021ec78de..5cc2290636157 100644 --- a/common/tier4_geography_utils/CMakeLists.txt +++ b/common/geography_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(tier4_geography_utils) +project(geography_utils) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,11 +12,11 @@ find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) find_library(GeographicLib_LIBRARIES NAMES Geographic) -ament_auto_add_library(tier4_geography_utils SHARED +ament_auto_add_library(geography_utils SHARED src/height.cpp ) -target_link_libraries(tier4_geography_utils +target_link_libraries(geography_utils ${GeographicLib_LIBRARIES} ) @@ -25,10 +25,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_tier4_geography_utils ${test_files}) + ament_add_ros_isolated_gtest(test_geography_utils ${test_files}) - target_link_libraries(test_tier4_geography_utils - tier4_geography_utils + target_link_libraries(test_geography_utils + geography_utils ) endif() diff --git a/common/tier4_geography_utils/README.md b/common/geography_utils/README.md similarity index 83% rename from common/tier4_geography_utils/README.md rename to common/geography_utils/README.md index 0c28cc7f9ad18..fb4c2dc3a8312 100644 --- a/common/tier4_geography_utils/README.md +++ b/common/geography_utils/README.md @@ -1,4 +1,4 @@ -# tier4_geography_utils +# geography_utils ## Purpose diff --git a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp similarity index 90% rename from common/tier4_geography_utils/include/tier4_geography_utils/height.hpp rename to common/geography_utils/include/geography_utils/height.hpp index 0fcb8f757049c..9657c62a82b08 100644 --- a/common/tier4_geography_utils/include/tier4_geography_utils/height.hpp +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_GEOGRAPHY_UTILS__HEIGHT_HPP_ -#define TIER4_GEOGRAPHY_UTILS__HEIGHT_HPP_ +#ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define GEOGRAPHY_UTILS__HEIGHT_HPP_ #include #include @@ -33,4 +33,4 @@ double convert_height( } // namespace tier4_geography_utils -#endif // TIER4_GEOGRAPHY_UTILS__HEIGHT_HPP_ +#endif // GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/tier4_geography_utils/package.xml b/common/geography_utils/package.xml similarity index 86% rename from common/tier4_geography_utils/package.xml rename to common/geography_utils/package.xml index 821b1c63eff28..61ac473498632 100644 --- a/common/tier4_geography_utils/package.xml +++ b/common/geography_utils/package.xml @@ -1,9 +1,9 @@ - tier4_geography_utils + geography_utils 0.1.0 - The tier4_geography_utils package + The geography_utils package Koji Minoda Apache License 2.0 diff --git a/common/tier4_geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp similarity index 98% rename from common/tier4_geography_utils/src/height.cpp rename to common/geography_utils/src/height.cpp index 287b168e11cbd..07023f3653608 100644 --- a/common/tier4_geography_utils/src/height.cpp +++ b/common/geography_utils/src/height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_geography_utils/height.hpp" +#include "geography_utils/height.hpp" #include diff --git a/common/tier4_geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp similarity index 88% rename from common/tier4_geography_utils/test/test_height.cpp rename to common/geography_utils/test/test_height.cpp index ba150a22c6c6e..e883d1ee63300 100644 --- a/common/tier4_geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_geography_utils/height.hpp" +#include "geography_utils/height.hpp" #include @@ -36,16 +36,16 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum) // Test case to verify valid source and target datums TEST(Tier4GeographyUtils, ValidSourceTargetDatum) { - const double height = 10.0; + const double height = 162.510695809; const double latitude = 35.0; const double longitude = 139.0; + const double target_height = -35; + double converted_height = tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); - // This is not a precise conversion - // As this is a conversion, just verify that it does not return the original height - EXPECT_NE(height, converted_height); + EXPECT_NEAR(target_height, converted_height, 0.1); } // Test case to verify invalid source and target datums diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index d1220b423da00..30131590b32d7 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 8544688343e98..97c32efd48c48 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -23,7 +23,7 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - tier4_geography_utils + geography_utils ament_lint_auto autoware_lint_common From 3701f7d7f949be707e8f3b174a7f0b1060009958 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:47:05 +0900 Subject: [PATCH 18/28] rename from tier4_geography_utils to geography_utils Signed-off-by: kminoda --- system/default_ad_api/package.xml | 2 +- system/default_ad_api/src/vehicle.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 119b90be05b65..7bdeb4aa0d91a 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -32,7 +32,7 @@ std_srvs tier4_api_msgs tier4_control_msgs - tier4_geography_utils + geography_utils tier4_system_msgs vehicle_info_util diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index ad228a05f2c9d..9a97094424611 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,7 +14,7 @@ #include "vehicle.hpp" -#include "tier4_geography_utils/height.hpp" +#include "geography_utils/height.hpp" #include From 60c0e01bf3fb5d50f519362f9733f44eb733eb00 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 06:49:07 +0000 Subject: [PATCH 19/28] style(pre-commit): autofix --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 2 +- sensing/gnss_poser/package.xml | 2 +- system/default_ad_api/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 30131590b32d7..8ea5681a5de7c 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -19,8 +19,8 @@ #include #include #include -#include #include +#include #include diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 97c32efd48c48..77380bf5492a1 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -15,6 +15,7 @@ autoware_sensing_msgs geographiclib + geography_utils geometry_msgs rclcpp rclcpp_components @@ -23,7 +24,6 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - geography_utils ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 7bdeb4aa0d91a..fd61d3c62e397 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -23,6 +23,7 @@ autoware_planning_msgs component_interface_specs component_interface_utils + geography_utils lanelet2_extension motion_utils nav_msgs @@ -32,7 +33,6 @@ std_srvs tier4_api_msgs tier4_control_msgs - geography_utils tier4_system_msgs vehicle_info_util From b9477f6bbfc339e2d0a9add4544c655c958d0c3f Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 15:55:17 +0900 Subject: [PATCH 20/28] add some test Signed-off-by: kminoda --- common/geography_utils/test/test_height.cpp | 32 ++++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index e883d1ee63300..022166870d1d0 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -36,11 +36,11 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum) // 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 = 162.510695809; - const double latitude = 35.0; - const double longitude = 139.0; - - const double target_height = -35; + const double latitude = 34.408092589; + const double longitude = -119.371255098; + const double target_height = 197.51; double converted_height = tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); @@ -60,6 +60,30 @@ TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) 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( + tier4_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( + tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + std::invalid_argument); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From 77b77bd915035ac1addf26fb592e9f91a519f146 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 06:57:18 +0000 Subject: [PATCH 21/28] style(pre-commit): autofix --- common/geography_utils/test/test_height.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index 022166870d1d0..70ec7a5067920 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -36,7 +36,8 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum) // 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 + // Calculated with + // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html const double height = 162.510695809; const double latitude = 34.408092589; const double longitude = -119.371255098; From 564abfbeb93b107d37e623767f83682f9fcb9ab8 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 16:00:04 +0900 Subject: [PATCH 22/28] edit test Signed-off-by: kminoda --- common/geography_utils/test/test_height.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index 70ec7a5067920..47e9b1a47d412 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -38,10 +38,10 @@ TEST(Tier4GeographyUtils, ValidSourceTargetDatum) { // Calculated with // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html - const double height = 162.510695809; - const double latitude = 34.408092589; - const double longitude = -119.371255098; - const double target_height = 197.51; + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const double target_height = -30.18; double converted_height = tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); From 68cd3d1baa06b09d6490638feba6b21d2aea2d3d Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 16:10:43 +0900 Subject: [PATCH 23/28] rename namespace Signed-off-by: kminoda --- common/geography_utils/test/test_height.cpp | 10 +++++----- sensing/gnss_poser/include/gnss_poser/convert.hpp | 8 ++++---- system/default_ad_api/src/vehicle.cpp | 4 ++-- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index 47e9b1a47d412..8b797e185e112 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -28,7 +28,7 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum) const std::string datum = "WGS84"; double converted_height = - tier4_geography_utils::convert_height(height, latitude, longitude, datum, datum); + geography_utils::convert_height(height, latitude, longitude, datum, datum); EXPECT_DOUBLE_EQ(height, converted_height); } @@ -44,7 +44,7 @@ TEST(Tier4GeographyUtils, ValidSourceTargetDatum) const double target_height = -30.18; double converted_height = - tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); EXPECT_NEAR(target_height, converted_height, 0.1); } @@ -57,7 +57,7 @@ TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) const double longitude = 139.0; EXPECT_THROW( - tier4_geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), std::invalid_argument); } @@ -69,7 +69,7 @@ TEST(Tier4GeographyUtils, InvalidSourceDatum) const double longitude = 139.0; EXPECT_THROW( - tier4_geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), std::invalid_argument); } @@ -81,7 +81,7 @@ TEST(Tier4GeographyUtils, InvalidTargetDatum) const double longitude = 139.0; EXPECT_THROW( - tier4_geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), std::invalid_argument); } diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 8ea5681a5de7c..624404de53fef 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -19,8 +19,8 @@ #include #include #include -#include #include +#include #include @@ -57,7 +57,7 @@ GNSSStat NavSatFix2UTM( } else { target_height_system = "WGS84"; } - utm.z = tier4_geography_utils::convert_height( + 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; @@ -86,7 +86,7 @@ GNSSStat NavSatFix2LocalCartesianUTM( } else { target_height_system = "WGS84"; } - utm_origin.z = tier4_geography_utils::convert_height( + 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); @@ -102,7 +102,7 @@ GNSSStat NavSatFix2LocalCartesianUTM( // individual coordinates of local coordinate system utm_local.x = global_x - utm_origin.x; utm_local.y = global_y - utm_origin.y; - utm_local.z = tier4_geography_utils::convert_height( + 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; diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 9a97094424611..33b238cad0ae6 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -155,7 +155,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = tier4_geography_utils::convert_height( + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, "WGS84"); } else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { @@ -169,7 +169,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = tier4_geography_utils::convert_height( + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, map_projector_info_->vertical_datum, "WGS84"); } else { From 8fe6e2ce89f7e66d31db0fc81e1c5091f17578d3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 07:12:28 +0000 Subject: [PATCH 24/28] style(pre-commit): autofix --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 624404de53fef..4dda5d1276c54 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -19,8 +19,8 @@ #include #include #include -#include #include +#include #include From 8698b223f2e17e881590a0d8f47f1a33ab314ef3 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 16:15:52 +0900 Subject: [PATCH 25/28] rename namespace complete Signed-off-by: kminoda --- .../include/geography_utils/height.hpp | 15 ++++++++++----- common/geography_utils/src/height.cpp | 10 ++++++---- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp index 9657c62a82b08..386f5820742af 100644 --- a/common/geography_utils/include/geography_utils/height.hpp +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -20,17 +20,22 @@ #include #include -namespace tier4_geography_utils +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); + 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 tier4_geography_utils +} // namespace geography_utils #endif // GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp index 07023f3653608..ecbb00bc1dd7f 100644 --- a/common/geography_utils/src/height.cpp +++ b/common/geography_utils/src/height.cpp @@ -21,17 +21,19 @@ #include #include -namespace tier4_geography_utils +namespace geography_utils { -double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) +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) +double convert_egm2008_to_wgs84( + const double height, const double latitude, const double longitude) { GeographicLib::Geoid egm2008("egm2008-1"); // cSpell: ignore GEOIDTOELLIPSOID @@ -61,4 +63,4 @@ double convert_height( } } -} // namespace tier4_geography_utils +} // namespace geography_utils From 283b351f69f573cf97b77b49a8879af607af80bc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 07:18:59 +0000 Subject: [PATCH 26/28] style(pre-commit): autofix --- .../include/geography_utils/height.hpp | 11 +++-------- common/geography_utils/src/height.cpp | 6 ++---- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp index 386f5820742af..1921e79699970 100644 --- a/common/geography_utils/include/geography_utils/height.hpp +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -24,14 +24,9 @@ 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); + 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); diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp index ecbb00bc1dd7f..49bb12165565e 100644 --- a/common/geography_utils/src/height.cpp +++ b/common/geography_utils/src/height.cpp @@ -24,16 +24,14 @@ namespace geography_utils { -double convert_wgs84_to_egm2008( - const double height, const double latitude, const double longitude) +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) +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) { GeographicLib::Geoid egm2008("egm2008-1"); // cSpell: ignore GEOIDTOELLIPSOID From 9068e1b4c3d1a7c09f229b5f9fdb067d8ddfe043 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 17:45:31 +0900 Subject: [PATCH 27/28] use angle brackets inclusion Signed-off-by: kminoda --- common/geography_utils/src/height.cpp | 2 +- common/geography_utils/test/test_height.cpp | 2 +- system/default_ad_api/src/vehicle.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp index 49bb12165565e..ad88c9588db27 100644 --- a/common/geography_utils/src/height.cpp +++ b/common/geography_utils/src/height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "geography_utils/height.hpp" +#include #include diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index 8b797e185e112..3d8a4c9c203fe 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "geography_utils/height.hpp" +#include #include diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 33b238cad0ae6..c8f7df315c487 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,7 +14,7 @@ #include "vehicle.hpp" -#include "geography_utils/height.hpp" +#include #include From caf9afa1996a19449e7d54ea7fa74a9c409e1b4f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 08:47:18 +0000 Subject: [PATCH 28/28] style(pre-commit): autofix --- common/geography_utils/src/height.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp index ad88c9588db27..fe69557f25a76 100644 --- a/common/geography_utils/src/height.cpp +++ b/common/geography_utils/src/height.cpp @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include #include #include