From 49645d80454382d3bee841d82a1dcf248d0ddc67 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 18 Aug 2023 16:29:55 +0900 Subject: [PATCH 01/42] feat(gnss_poser): Subscribe map_projector_info Signed-off-by: kminoda --- .../include/gnss_poser/gnss_poser_core.hpp | 11 ++++- sensing/gnss_poser/package.xml | 2 + sensing/gnss_poser/src/gnss_poser_core.cpp | 41 ++++++++++++++++--- 3 files changed, 48 insertions(+), 6 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 2b729a9e374f7..6cab315f1ab64 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -18,12 +18,14 @@ #include "gnss_poser/gnss_stat.hpp" #include - +#include +#include #include #include #include #include #include +#include #include @@ -42,12 +44,17 @@ namespace gnss_poser { +CoordinateSystem convert_to_coordinate_systems(const std::string & type); + class GNSSPoser : public rclcpp::Node { public: explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + + void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); void callbackGnssInsOrientationStamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); @@ -81,6 +88,7 @@ class GNSSPoser : public rclcpp::Node tf2_ros::TransformListener tf2_listener_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; rclcpp::Subscription::SharedPtr autoware_orientation_sub_; @@ -94,6 +102,7 @@ class GNSSPoser : public rclcpp::Node std::string gnss_frame_; std::string gnss_base_frame_; std::string map_frame_; + bool received_map_projector_info_ = false; sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; bool use_gnss_ins_orientation_; diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 4887494614a81..8e066ed0dd384 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -24,6 +24,8 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs + component_interface_utils + component_interface_specs ament_lint_auto autoware_lint_common diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 8fc7d9c1599f5..34297cd1f4837 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -23,6 +23,20 @@ namespace gnss_poser { +CoordinateSystem convert_to_coordinate_systems(const std::string & type) +{ + if (type == "MGRS") { + return CoordinateSystem::MGRS; + } else if (type == "UTM") { + return CoordinateSystem::UTM; + } else if (type == "local") { + return CoordinateSystem::PLANE; + } else if (type == "local_cartesian_wgs84") { + return CoordinateSystem::LOCAL_CARTESIAN_WGS84; + } + return CoordinateSystem::PLANE; +} + GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) : rclcpp::Node("gnss_poser", node_options), tf2_listener_(tf2_buffer_), @@ -38,12 +52,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) height_system_(declare_parameter("height_system", 1)), gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) { - int coordinate_system = - declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); - coordinate_system_ = static_cast(coordinate_system); + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + qos.transient_local(); // Set Durability QoS policy to transient local. + + // Subscribe to map_projector_info topic + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub(sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); - nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0); - nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0); nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); int buff_epoch = declare_parameter("buff_epoch", 1); @@ -62,9 +77,25 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); } +void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +{ + coordinate_system_ = convert_to_coordinate_systems(msg->type); + this->nav_sat_fix_origin_.latitude = msg->map_origin.latitude; + this->nav_sat_fix_origin_.longitude = msg->map_origin.longitude; + received_map_projector_info_ = true; +} + void GNSSPoser::callbackNavSatFix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { + // Return immediately if map_projector_info has not been received yet. + if (!received_map_projector_info_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "map_projector_info has not been received yet. Skipping Calculate."); + return; + } + // check fixed topic const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); From 5cc24bedc874615d9ee6577b7dfbd28efee7eacf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 18 Aug 2023 07:31:42 +0000 Subject: [PATCH 02/42] style(pre-commit): autofix --- sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp | 3 ++- sensing/gnss_poser/package.xml | 4 ++-- sensing/gnss_poser/src/gnss_poser_core.cpp | 4 +++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 6cab315f1ab64..61719af76a5ce 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -17,9 +17,10 @@ #include "gnss_poser/convert.hpp" #include "gnss_poser/gnss_stat.hpp" -#include #include #include +#include + #include #include #include diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 8e066ed0dd384..68dbfa2be4ef3 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -14,6 +14,8 @@ libboost-dev autoware_sensing_msgs + component_interface_specs + component_interface_utils geo_pos_conv geographiclib geometry_msgs @@ -24,8 +26,6 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - component_interface_utils - component_interface_specs ament_lint_auto autoware_lint_common diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 34297cd1f4837..39af29a00a89f 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -57,7 +57,9 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); - adaptor.init_sub(sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); From 36bb470515098bd7889195eca52b94c42fc9481c Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 18 Aug 2023 16:39:03 +0900 Subject: [PATCH 03/42] update readme Signed-off-by: kminoda --- sensing/gnss_poser/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index d37152297c61d..92ed4cc6b6191 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -12,6 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | From 59bceb587bcd6b36243aad5b81155e0c4dd1c7a0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 18 Aug 2023 07:42:06 +0000 Subject: [PATCH 04/42] style(pre-commit): autofix --- sensing/gnss_poser/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 92ed4cc6b6191..a108d75a0f6d5 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -12,7 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | -| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | From fd945d326a527b472cf5d4e4d4a756340e2103a9 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Aug 2023 13:42:24 +0900 Subject: [PATCH 05/42] small fix Signed-off-by: kminoda --- sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp | 1 - sensing/gnss_poser/src/gnss_poser_core.cpp | 5 +---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 61719af76a5ce..3667c5fcc5dc5 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -26,7 +26,6 @@ #include #include #include -#include #include diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 39af29a00a89f..c3bddfa180a40 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -52,9 +52,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) height_system_(declare_parameter("height_system", 1)), gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) { - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); - qos.transient_local(); // Set Durability QoS policy to transient local. - // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); adaptor.init_sub( @@ -94,7 +91,7 @@ void GNSSPoser::callbackNavSatFix( if (!received_map_projector_info_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "map_projector_info has not been received yet. Skipping Calculate."); + "map_projector_info has not been received yet. Skipping calculation."); return; } From a87052ecc280425ce9061df2cde0753b7ced0651 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Aug 2023 13:46:54 +0900 Subject: [PATCH 06/42] update commetn Signed-off-by: kminoda --- sensing/gnss_poser/src/gnss_poser_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index c3bddfa180a40..d6f210501a68f 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -91,7 +91,7 @@ void GNSSPoser::callbackNavSatFix( if (!received_map_projector_info_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "map_projector_info has not been received yet. Skipping calculation."); + "map_projector_info has not been received yet. Check if the map_projection_loader is successfully launched."); return; } From 1e8baec8ac9bc378b952604e9ba3abbdd99571eb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 22 Aug 2023 04:48:32 +0000 Subject: [PATCH 07/42] style(pre-commit): autofix --- sensing/gnss_poser/src/gnss_poser_core.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index d6f210501a68f..3f0102b8972af 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -91,7 +91,8 @@ void GNSSPoser::callbackNavSatFix( if (!received_map_projector_info_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "map_projector_info has not been received yet. Check if the map_projection_loader is successfully launched."); + "map_projector_info has not been received yet. Check if the map_projection_loader is " + "successfully launched."); return; } From cd1cc72d3251cf1a1a2b5174256d367932edb8b4 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Aug 2023 14:19:52 +0900 Subject: [PATCH 08/42] add local cartesian Signed-off-by: kminoda --- sensing/gnss_poser/src/gnss_poser_core.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 3f0102b8972af..890e46c6f5349 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -31,6 +31,8 @@ CoordinateSystem convert_to_coordinate_systems(const std::string & type) return CoordinateSystem::UTM; } else if (type == "local") { return CoordinateSystem::PLANE; + } else if (type == "local_cartesian_utm") { + return CoordinateSystem::LOCAL_CARTESIAN_UTM; } else if (type == "local_cartesian_wgs84") { return CoordinateSystem::LOCAL_CARTESIAN_WGS84; } @@ -224,14 +226,14 @@ GNSSStat GNSSPoser::convert( GNSSStat gnss_stat; if (coordinate_system == CoordinateSystem::UTM) { gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { - gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); } else if (coordinate_system == CoordinateSystem::MGRS) { gnss_stat = NavSatFix2MGRS( nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system); } else if (coordinate_system == CoordinateSystem::PLANE) { gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger()); + } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { + gnss_stat = NavSatFix2LocalCartesianUTM( + nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) { gnss_stat = NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); From 0c9cf23b6ad1771071e6bcc271ee0a287fca1e0a Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Aug 2023 14:20:31 +0900 Subject: [PATCH 09/42] update launch file Signed-off-by: kminoda --- sensing/gnss_poser/launch/gnss_poser.launch.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index e7dc06864ce84..5d995d3fc547b 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -13,7 +13,6 @@ - @@ -32,7 +31,6 @@ - From f0f9bb345c6ed25cfdd54989fc1c1703204069ab Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 29 Aug 2023 02:55:55 +0000 Subject: [PATCH 10/42] style(pre-commit): autofix --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 3 ++- sensing/gnss_poser/src/gnss_poser_core.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index deb01517e65ee..df4b538d49794 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -81,7 +81,8 @@ GNSSStat NavSatFix2UTM( } GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, const std::string & vertical_datum) + sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, + const std::string & vertical_datum) { GNSSStat utm_local; try { diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index a98715bdfe2cb..af658335cc50b 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -95,7 +95,8 @@ void GNSSPoser::callbackNavSatFix( } // get position - const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_.projector_type, projector_info_.vertical_datum); + const auto gnss_stat = + convert(*nav_sat_fix_msg_ptr, projector_info_.projector_type, projector_info_.vertical_datum); const auto position = getPosition(gnss_stat); geometry_msgs::msg::Pose gnss_antenna_pose{}; From 8b53bbcf9a3f1ddab74d540aae1cf8b479ac1945 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 29 Aug 2023 14:43:17 +0900 Subject: [PATCH 11/42] fix Signed-off-by: kminoda --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index df4b538d49794..6b47eafd8ba32 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -68,8 +68,10 @@ GNSSStat NavSatFix2UTM( utm.y); if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::WGS84) { utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); - } else { + } else if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::EGM2008) { utm.z = nav_sat_fix_msg.altitude; + } else { + RCLCPP_ERROR_STREAM(logger, "Invalid vertical datum type: " << vertical_datum.c_str()); } utm.latitude = nav_sat_fix_msg.latitude; utm.longitude = nav_sat_fix_msg.longitude; From 9a7655beecd3c08bab73fc38e4f8577c390afad8 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 29 Aug 2023 15:11:06 +0900 Subject: [PATCH 12/42] create new function for conversion of height Signed-off-by: kminoda --- .../gnss_poser/include/gnss_poser/convert.hpp | 51 +++++++------------ 1 file changed, 19 insertions(+), 32 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 6b47eafd8ba32..07a60af0dfc83 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -39,23 +39,24 @@ 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 convertHeight2EGM2008( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const std::string & vertical_datum) { - double OrthometricHeight{0.0}; - try { + double height{0.0}; + if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::WGS84) { GeographicLib::Geoid egm2008("egm2008-1"); - OrthometricHeight = egm2008.ConvertHeight( + height = 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()); + } else if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::EGM2008) { + height = nav_sat_fix_msg.altitude; + } else { + throw std::runtime_error("Invalid vertical datum type: " + vertical_datum); } - return OrthometricHeight; + return height; } + GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, const std::string & vertical_datum) @@ -66,18 +67,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); - if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::WGS84) { - utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); - } else if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::EGM2008) { - utm.z = nav_sat_fix_msg.altitude; - } else { - RCLCPP_ERROR_STREAM(logger, "Invalid vertical datum type: " << vertical_datum.c_str()); - } + utm.z = convertHeight2EGM2008(nav_sat_fix_msg, vertical_datum); 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()); + } catch (const std::runtime_error & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert height. " << err.what()); } return utm; } @@ -93,13 +90,7 @@ 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); - if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::WGS84) { - utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger); - } else if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::EGM2008) { - utm_origin.z = nav_sat_fix_origin.altitude; - } else { - RCLCPP_ERROR_STREAM(logger, "Invalid vertical datum type: " << vertical_datum.c_str()); - } + utm_origin.z = convertHeight2EGM2008(nav_sat_fix_origin, vertical_datum); // individual coordinates of global coordinate system double global_x = 0.0; @@ -113,16 +104,12 @@ 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 (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::WGS84) { - utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z; - } else if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::EGM2008) { - utm_local.z = nav_sat_fix_msg.altitude - utm_origin.z; - } else { - RCLCPP_ERROR_STREAM(logger, "Invalid vertical datum type: " << vertical_datum.c_str()); - } + utm_local.z = convertHeight2EGM2008(nav_sat_fix_msg, vertical_datum) - utm_origin.z; } catch (const GeographicLib::GeographicErr & err) { RCLCPP_ERROR_STREAM( logger, "Failed to convert from LLH to UTM in local coordinates" << err.what()); + } catch (const std::runtime_error & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert height. " << err.what()); } return utm_local; } From 2fb5d4ecc4cc6866a1845c5386fa870f5e47f50b Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 29 Aug 2023 15:14:34 +0900 Subject: [PATCH 13/42] minor update Signed-off-by: kminoda --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 6 +++--- 1 file 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 07a60af0dfc83..822a3a3548ee4 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include @@ -44,12 +44,12 @@ double convertHeight2EGM2008( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const std::string & vertical_datum) { double height{0.0}; - if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::WGS84) { + if (vertical_datum == map_interface::MapProjectorInfo::Message::WGS84) { GeographicLib::Geoid egm2008("egm2008-1"); height = egm2008.ConvertHeight( nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, GeographicLib::Geoid::ELLIPSOIDTOGEOID); - } else if (vertical_datum == tier4_map_msgs::msg::MapProjectorInfo::EGM2008) { + } else if (vertical_datum == map_interface::MapProjectorInfo::Message::EGM2008) { height = nav_sat_fix_msg.altitude; } else { throw std::runtime_error("Invalid vertical datum type: " + vertical_datum); From bd5f31c810676aea05fff4ee36fb537739148062 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 29 Aug 2023 06:16:19 +0000 Subject: [PATCH 14/42] 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 822a3a3548ee4..129469c246b51 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -19,10 +19,10 @@ #include #include #include +#include #include #include -#include #include From 5113d16e1dd175feaa16d0f8b98d436d50628055 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 29 Aug 2023 15:35:18 +0900 Subject: [PATCH 15/42] update Signed-off-by: kminoda --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 129469c246b51..17790e99bcff8 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -73,8 +73,6 @@ GNSSStat NavSatFix2UTM( 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()); - } catch (const std::runtime_error & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert height. " << err.what()); } return utm; } @@ -108,8 +106,6 @@ GNSSStat NavSatFix2LocalCartesianUTM( } catch (const GeographicLib::GeographicErr & err) { RCLCPP_ERROR_STREAM( logger, "Failed to convert from LLH to UTM in local coordinates" << err.what()); - } catch (const std::runtime_error & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert height. " << err.what()); } return utm_local; } From 8b6bca06d51bf0b273c44129283c884a0c4c7784 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 21:25:48 +0900 Subject: [PATCH 16/42] rename Signed-off-by: kminoda --- .../gnss_poser/include/gnss_poser/convert.hpp | 22 +++++-------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 317cca566721c..6fba4d560edb9 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -43,7 +43,7 @@ enum class MGRSPrecision { GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - const std::string & vertical_datum) + const std::string & target_vertical_datum) { GNSSStat utm; @@ -51,15 +51,9 @@ 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 = geography_utils::convert_height( nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", - target_height_system); + target_vertical_datum); utm.latitude = nav_sat_fix_msg.latitude; utm.longitude = nav_sat_fix_msg.longitude; utm.altitude = nav_sat_fix_msg.altitude; @@ -72,7 +66,7 @@ GNSSStat NavSatFix2UTM( GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, - const std::string & vertical_datum) + const std::string & target_vertical_datum) { GNSSStat utm_local; try { @@ -82,15 +76,9 @@ GNSSStat NavSatFix2LocalCartesianUTM( 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 = geography_utils::convert_height( nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, - "WGS84", target_height_system); + "WGS84", target_vertical_datum); // individual coordinates of global coordinate system double global_x = 0.0; @@ -106,7 +94,7 @@ GNSSStat NavSatFix2LocalCartesianUTM( utm_local.y = global_y - utm_origin.y; 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) - + "WGS84", target_vertical_datum) - utm_origin.z; } catch (const GeographicLib::GeographicErr & err) { RCLCPP_ERROR_STREAM( From 8a5c2b86d6ad8ce0f4ab5a29746a5dfbe6e66828 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 30 Aug 2023 21:40:58 +0900 Subject: [PATCH 17/42] remove unnecessary include Signed-off-by: kminoda --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 6fba4d560edb9..6e5f2f1ef989d 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include From 3363af00169e0350e9c2d800819dcee7a46f4649 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 31 Aug 2023 15:15:02 +0900 Subject: [PATCH 18/42] add projection in geography_utils Signed-off-by: kminoda --- common/geography_utils/CMakeLists.txt | 1 + .../geography_utils/geography_utils.hpp | 21 +++++ .../include/geography_utils/projection.hpp | 33 +++++++ common/geography_utils/package.xml | 4 + common/geography_utils/src/projection.cpp | 93 +++++++++++++++++++ 5 files changed, 152 insertions(+) create mode 100644 common/geography_utils/include/geography_utils/geography_utils.hpp create mode 100644 common/geography_utils/include/geography_utils/projection.hpp create mode 100644 common/geography_utils/src/projection.cpp diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt index 5cc2290636157..9c38486f29c93 100644 --- a/common/geography_utils/CMakeLists.txt +++ b/common/geography_utils/CMakeLists.txt @@ -14,6 +14,7 @@ find_library(GeographicLib_LIBRARIES NAMES Geographic) ament_auto_add_library(geography_utils SHARED src/height.cpp + src/projection.cpp ) target_link_libraries(geography_utils diff --git a/common/geography_utils/include/geography_utils/geography_utils.hpp b/common/geography_utils/include/geography_utils/geography_utils.hpp new file mode 100644 index 0000000000000..8dee8d27dbacd --- /dev/null +++ b/common/geography_utils/include/geography_utils/geography_utils.hpp @@ -0,0 +1,21 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOGRAPHY_UTILS__GEOGRAPHY_UTILS_HPP_ +#define GEOGRAPHY_UTILS__GEOGRAPHY_UTILS_HPP_ + +#include "geography_utils/height.hpp" +#include "geography_utils/projection.hpp" + +#endif // GEOGRAPHY_UTILS__GEOGRAPHY_UTILS_HPP_ diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/geography_utils/include/geography_utils/projection.hpp new file mode 100644 index 0000000000000..137989482201b --- /dev/null +++ b/common/geography_utils/include/geography_utils/projection.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOGRAPHY_UTILS__PROJECTION_HPP_ +#define GEOGRAPHY_UTILS__PROJECTION_HPP_ + +#include +#include +#include + +namespace geography_utils +{ +using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; +using GeoPoint = geographic_msgs::msg::GeoPoint; +using LocalPoint = geometry_msgs::msg::Point; + +LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & projector_info); +GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & projector_info); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml index 61ac473498632..19832702b13db 100644 --- a/common/geography_utils/package.xml +++ b/common/geography_utils/package.xml @@ -11,6 +11,10 @@ autoware_cmake geographiclib + tier4_map_msgs + geometry_msgs + lanelet2_projection + lanelet2_extension ament_cmake_ros ament_lint_auto diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp new file mode 100644 index 0000000000000..a02b51f814b43 --- /dev/null +++ b/common/geography_utils/src/projection.cpp @@ -0,0 +1,93 @@ +// 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 +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace geography_utils +{ + +Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) +{ + Eigen::Vector3d dst; + dst.x() = src.x; + dst.y() = src.y; + dst.z() = src.z; + return dst; +} + +LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & projector_info) +{ + LocalPoint local_point; + if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, 0.0}; + lanelet::Origin origin{position}; + lanelet::projection::UtmProjector projector{origin}; + lanelet::BasicPoint3d projected_local_point = projector.forward(position); + local_point.x = projected_local_point.x(); + local_point.y = projected_local_point.y(); + local_point.z = std::numeric_limits::quiet_NaN(); + } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const int mgrs_precision = 9; // set precision as 100 micro meter + lanelet::projection::MGRSProjector projector{}; + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, 0.0}; + lanelet::BasicPoint3d projected_local_point = projector.forward(position, mgrs_precision); + local_point.x = projected_local_point.x(); + local_point.y = projected_local_point.y(); + local_point.z = std::numeric_limits::quiet_NaN(); + } else { + throw std::runtime_error( + "Invalid map projector type. Currently supported types: MGRS, and LocalCartesianUTM"); + } + return local_point; +} + +GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & projector_info) +{ + GeoPoint geo_point; + if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude}; + lanelet::Origin origin{position}; + lanelet::projection::UtmProjector projector{origin}; + lanelet::GPSPoint projected_gps_point = + projector.reverse(to_basic_point_3d_pt(local_point)); + geo_point.latitude = projected_gps_point.lat; + geo_point.longitude = projected_gps_point.lon; + geo_point.altitude = std::numeric_limits::quiet_NaN(); + + } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse( + to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); + geo_point.latitude = projected_gps_point.lat; + geo_point.longitude = projected_gps_point.lon; + geo_point.altitude = std::numeric_limits::quiet_NaN(); + + } else { + throw std::runtime_error( + "Invalid map projector type. Currently supported types: MGRS, and LocalCartesianUTM"); + } + return geo_point; +} + +} // namespace geography_utils From cd3771eae99d5d92d77bcc3687a0c7995439ca29 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 31 Aug 2023 15:42:01 +0900 Subject: [PATCH 19/42] update projection.cpp Signed-off-by: kminoda --- common/geography_utils/src/projection.cpp | 26 +++++++++++------------ 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index a02b51f814b43..9710aca70caee 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -40,24 +40,25 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr { LocalPoint local_point; if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, 0.0}; + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; lanelet::BasicPoint3d projected_local_point = projector.forward(position); local_point.x = projected_local_point.x(); local_point.y = projected_local_point.y(); - local_point.z = std::numeric_limits::quiet_NaN(); + local_point.z = projected_local_point.z(); } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { const int mgrs_precision = 9; // set precision as 100 micro meter lanelet::projection::MGRSProjector projector{}; - lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, 0.0}; + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; lanelet::BasicPoint3d projected_local_point = projector.forward(position, mgrs_precision); local_point.x = projected_local_point.x(); local_point.y = projected_local_point.y(); - local_point.z = std::numeric_limits::quiet_NaN(); + local_point.z = projected_local_point.z(); } else { - throw std::runtime_error( - "Invalid map projector type. Currently supported types: MGRS, and LocalCartesianUTM"); + const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, and LocalCartesianUTM"; + throw std::runtime_error(error_msg); } return local_point; } @@ -67,25 +68,24 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & GeoPoint geo_point; if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { lanelet::GPSPoint position{ - projector_info.map_origin.latitude, projector_info.map_origin.longitude}; + projector_info.map_origin.latitude, projector_info.map_origin.longitude, projector_info.map_origin.altitude}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; lanelet::GPSPoint projected_gps_point = projector.reverse(to_basic_point_3d_pt(local_point)); geo_point.latitude = projected_gps_point.lat; geo_point.longitude = projected_gps_point.lon; - geo_point.altitude = std::numeric_limits::quiet_NaN(); - + geo_point.altitude = projected_gps_point.ele; } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse( to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); geo_point.latitude = projected_gps_point.lat; geo_point.longitude = projected_gps_point.lon; - geo_point.altitude = std::numeric_limits::quiet_NaN(); - + geo_point.altitude = projected_gps_point.ele; } else { - throw std::runtime_error( - "Invalid map projector type. Currently supported types: MGRS, and LocalCartesianUTM"); + const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, and LocalCartesianUTM"; + throw std::runtime_error(error_msg); } return geo_point; } From 4c51b73d1f819cb381f0eba44e5c65696235b341 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 31 Aug 2023 15:42:16 +0900 Subject: [PATCH 20/42] use projection in default_ad_api Signed-off-by: kminoda --- system/default_ad_api/package.xml | 1 + system/default_ad_api/src/vehicle.cpp | 28 ++++++++------------------- 2 files changed, 9 insertions(+), 20 deletions(-) diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index fd61d3c62e397..fadcccdd91268 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -24,6 +24,7 @@ component_interface_specs component_interface_utils geography_utils + geographic_msgs lanelet2_extension motion_utils nav_msgs diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index c8f7df315c487..ab342c20c6518 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -15,6 +15,8 @@ #include "vehicle.hpp" #include +#include +#include #include @@ -148,29 +150,15 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.twist.header = kinematic_state_msgs_->header; vehicle_kinematics.twist.header.frame_id = kinematic_state_msgs_->child_frame_id; vehicle_kinematics.twist.twist = kinematic_state_msgs_->twist; - if (map_projector_info_->projector_type == MapProjectorInfo::MGRS) { - lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse( - toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position), map_projector_info_->mgrs_grid); + if (map_projector_info_->projector_type != MapProjectorInfo::LOCAL) { + const geographic_msgs::msg::GeoPoint projected_gps_point = geography_utils::project_reverse( + kinematic_state_msgs_->pose.pose.position, *map_projector_info_); vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; 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.latitude = projected_gps_point.latitude; + vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.longitude; 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) { - lanelet::GPSPoint position{ - map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - lanelet::GPSPoint projected_gps_point = - projector.reverse(toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position)); - vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; - 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 = geography_utils::convert_height( - projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, + projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, map_projector_info_->vertical_datum, "WGS84"); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); From 13356fe6176bd19be353b1949b2b0b87954e714c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 31 Aug 2023 07:01:59 +0000 Subject: [PATCH 21/42] style(pre-commit): autofix --- .../include/geography_utils/projection.hpp | 2 +- common/geography_utils/package.xml | 4 ++-- common/geography_utils/src/projection.cpp | 14 +++++++------- system/default_ad_api/package.xml | 2 +- system/default_ad_api/src/vehicle.cpp | 1 + 5 files changed, 12 insertions(+), 11 deletions(-) diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/geography_utils/include/geography_utils/projection.hpp index 137989482201b..6b74e06532b49 100644 --- a/common/geography_utils/include/geography_utils/projection.hpp +++ b/common/geography_utils/include/geography_utils/projection.hpp @@ -15,9 +15,9 @@ #ifndef GEOGRAPHY_UTILS__PROJECTION_HPP_ #define GEOGRAPHY_UTILS__PROJECTION_HPP_ -#include #include #include +#include namespace geography_utils { diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml index 19832702b13db..3a07f2935ee60 100644 --- a/common/geography_utils/package.xml +++ b/common/geography_utils/package.xml @@ -11,10 +11,10 @@ autoware_cmake geographiclib - tier4_map_msgs geometry_msgs - lanelet2_projection lanelet2_extension + lanelet2_projection + tier4_map_msgs ament_cmake_ros ament_lint_auto diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index 9710aca70caee..e542d69426387 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -14,10 +14,10 @@ #include #include +#include #include #include -#include #include #include @@ -48,7 +48,7 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr local_point.y = projected_local_point.y(); local_point.z = projected_local_point.z(); } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { - const int mgrs_precision = 9; // set precision as 100 micro meter + const int mgrs_precision = 9; // set precision as 100 micro meter lanelet::projection::MGRSProjector projector{}; lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; lanelet::BasicPoint3d projected_local_point = projector.forward(position, mgrs_precision); @@ -57,7 +57,7 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr local_point.z = projected_local_point.z(); } else { const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, and LocalCartesianUTM"; + ". Currently supported types: MGRS, and LocalCartesianUTM"; throw std::runtime_error(error_msg); } return local_point; @@ -68,11 +68,11 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & GeoPoint geo_point; if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { lanelet::GPSPoint position{ - projector_info.map_origin.latitude, projector_info.map_origin.longitude, projector_info.map_origin.altitude}; + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; - lanelet::GPSPoint projected_gps_point = - projector.reverse(to_basic_point_3d_pt(local_point)); + lanelet::GPSPoint projected_gps_point = projector.reverse(to_basic_point_3d_pt(local_point)); geo_point.latitude = projected_gps_point.lat; geo_point.longitude = projected_gps_point.lon; geo_point.altitude = projected_gps_point.ele; @@ -84,7 +84,7 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & geo_point.altitude = projected_gps_point.ele; } else { const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, and LocalCartesianUTM"; + ". Currently supported types: MGRS, and LocalCartesianUTM"; throw std::runtime_error(error_msg); } return geo_point; diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index fadcccdd91268..9af361739129e 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -23,8 +23,8 @@ autoware_planning_msgs component_interface_specs component_interface_utils - geography_utils geographic_msgs + geography_utils lanelet2_extension motion_utils nav_msgs diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index ab342c20c6518..7f211e106988d 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -16,6 +16,7 @@ #include #include + #include #include From fe4ec419b635bbe87c4c9da943f3c656d8a55217 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 1 Sep 2023 10:02:15 +0900 Subject: [PATCH 22/42] Feat/gnss poser/subscribe map projector info (#24) * fix map origin Signed-off-by: kminoda * style(pre-commit): autofix * remove config file Signed-off-by: kminoda * rfix readme Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/gnss_poser/CMakeLists.txt | 1 - sensing/gnss_poser/README.md | 1 - .../config/set_local_origin.param.yaml | 11 ----------- .../gnss_poser/include/gnss_poser/convert.hpp | 9 +++++---- .../include/gnss_poser/gnss_poser_core.hpp | 6 ++---- .../gnss_poser/launch/gnss_poser.launch.xml | 2 -- sensing/gnss_poser/package.xml | 1 + sensing/gnss_poser/src/gnss_poser_core.cpp | 19 +++++++++---------- 8 files changed, 17 insertions(+), 33 deletions(-) delete mode 100644 sensing/gnss_poser/config/set_local_origin.param.yaml diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 993cf2d9da9e1..bd9886d376cad 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -37,5 +37,4 @@ rclcpp_components_register_node(gnss_poser_node ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 03554c927999f..b271f75eeeb6c 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -34,7 +34,6 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | `gnss_frame` | string | "gnss" | frame id | | `gnss_base_frame` | string | "gnss_base_link" | frame id | | `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 4: UTM Local Coordinate System | | `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml deleted file mode 100644 index e931e5adceb68..0000000000000 --- a/sensing/gnss_poser/config/set_local_origin.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - - # Latitude of local origin - latitude: 40.81187906 - - # Longitude of local origin - longitude: 29.35810110 - - # Altitude of local origin - altitude: 47.62 diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 6e5f2f1ef989d..5cd0767f28130 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 #include @@ -64,7 +65,7 @@ GNSSStat NavSatFix2UTM( GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, + const geographic_msgs::msg::GeoPoint geo_point_origin, const rclcpp::Logger & logger, const std::string & target_vertical_datum) { GNSSStat utm_local; @@ -72,12 +73,12 @@ GNSSStat NavSatFix2LocalCartesianUTM( // 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, + geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone, utm_origin.east_north_up, utm_origin.x, utm_origin.y); utm_origin.z = geography_utils::convert_height( - nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, - "WGS84", target_vertical_datum); + geo_point_origin.altitude, geo_point_origin.latitude, geo_point_origin.longitude, "WGS84", + target_vertical_datum); // individual coordinates of global coordinate system double global_x = 0.0; diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index fb3cce9406c94..05acfa967eb96 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -60,8 +60,8 @@ class GNSSPoser : public rclcpp::Node bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); GNSSStat convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const std::string & projector_type, - const std::string & vertical_datum); + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + const MapProjectorInfo::Message & projector_info); geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); geometry_msgs::msg::Point getMedianPosition( const boost::circular_buffer & position_buffer); @@ -101,8 +101,6 @@ class GNSSPoser : public rclcpp::Node std::string gnss_base_frame_; std::string map_frame_; bool received_map_projector_info_ = false; - - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; bool use_gnss_ins_orientation_; boost::circular_buffer position_buffer_; diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index f258297053d50..60e952c6845f4 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -2,7 +2,6 @@ - @@ -31,7 +30,6 @@ - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index ebce55b2c0d71..7d80bc903a550 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -16,6 +16,7 @@ autoware_sensing_msgs component_interface_specs component_interface_utils + geographic_msgs geographiclib geography_utils geometry_msgs diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index af658335cc50b..9cd5a3ad34098 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -42,8 +42,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); - nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); - int buff_epoch = declare_parameter("buff_epoch", 1); position_buffer_.set_capacity(buff_epoch); @@ -95,8 +93,7 @@ void GNSSPoser::callbackNavSatFix( } // get position - const auto gnss_stat = - convert(*nav_sat_fix_msg_ptr, projector_info_.projector_type, projector_info_.vertical_datum); + const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_); const auto position = getPosition(gnss_stat); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -201,16 +198,18 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix } GNSSStat GNSSPoser::convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const std::string & projector_type, - const std::string & vertical_datum) + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + const MapProjectorInfo::Message & map_projector_info) { GNSSStat gnss_stat; - if (projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { + if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), vertical_datum); - } else if (projector_type == MapProjectorInfo::Message::MGRS) { + nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(), + map_projector_info.vertical_datum); + } else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) { gnss_stat = NavSatFix2MGRS( - nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), vertical_datum); + nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), + map_projector_info.vertical_datum); } else { RCLCPP_ERROR_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), From 0bbba344010d1023c4564a8711a07917ac02652f Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 10:25:08 +0900 Subject: [PATCH 23/42] use projection.cpp in gnss_poser Signed-off-by: kminoda --- .../include/gnss_poser/gnss_poser_core.hpp | 22 +++--- sensing/gnss_poser/src/gnss_poser_core.cpp | 72 ++++++++++--------- 2 files changed, 51 insertions(+), 43 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 05acfa967eb96..93d81c8db194c 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -14,8 +14,8 @@ #ifndef GNSS_POSER__GNSS_POSER_CORE_HPP_ #define GNSS_POSER__GNSS_POSER_CORE_HPP_ -#include "gnss_poser/convert.hpp" -#include "gnss_poser/gnss_stat.hpp" +// #include "gnss_poser/convert.hpp" +// #include "gnss_poser/gnss_stat.hpp" #include #include @@ -51,6 +51,7 @@ class GNSSPoser : public rclcpp::Node private: using MapProjectorInfo = map_interface::MapProjectorInfo; + using Point = geometry_msgs::msg::Point; void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); @@ -59,17 +60,16 @@ class GNSSPoser : public rclcpp::Node bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - GNSSStat convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - const MapProjectorInfo::Message & projector_info); - geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); - geometry_msgs::msg::Point getMedianPosition( - const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Point getAveragePosition( - const boost::circular_buffer & position_buffer); + // GNSSStat convert( + // const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + // const MapProjectorInfo::Message & projector_info); + Point getMedianPosition( + const boost::circular_buffer & position_buffer); + Point getAveragePosition( + const boost::circular_buffer & position_buffer); geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); + const Point & point, const Point & prev_point); bool getTransform( const std::string & target_frame, const std::string & source_frame, diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 9cd5a3ad34098..15db73fa7cc31 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -15,7 +15,8 @@ #include "gnss_poser/gnss_poser_core.hpp" #include - +#include +#include #include #include #include @@ -93,8 +94,14 @@ void GNSSPoser::callbackNavSatFix( } // get position - const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_); - const auto position = getPosition(gnss_stat); + geographic_msgs::msg::GeoPoint gps_point; + gps_point.latitude = nav_sat_fix_msg_ptr->latitude; + gps_point.longitude = nav_sat_fix_msg_ptr->longitude; + gps_point.altitude = nav_sat_fix_msg_ptr->altitude; + Point position = geography_utils::project_forward(gps_point, projector_info_); + position.z = geography_utils::convert_height( + gps_point.altitude, gps_point.latitude, gps_point.longitude, + "WGS84", projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -197,35 +204,36 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -GNSSStat GNSSPoser::convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - const MapProjectorInfo::Message & map_projector_info) -{ - GNSSStat gnss_stat; - if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { - gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(), - map_projector_info.vertical_datum); - } else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) { - gnss_stat = NavSatFix2MGRS( - nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), - map_projector_info.vertical_datum); - } else { - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Unknown Projector type"); - } - return gnss_stat; -} - -geometry_msgs::msg::Point GNSSPoser::getPosition(const GNSSStat & gnss_stat) -{ - geometry_msgs::msg::Point point; - point.x = gnss_stat.x; - point.y = gnss_stat.y; - point.z = gnss_stat.z; - return point; -} +// GNSSStat GNSSPoser::convert( +// const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, +// const MapProjectorInfo::Message & map_projector_info) +// { +// GNSSStat gnss_stat; + +// if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { +// gnss_stat = NavSatFix2LocalCartesianUTM( +// nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(), +// map_projector_info.vertical_datum); +// } else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) { +// gnss_stat = NavSatFix2MGRS( +// nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), +// map_projector_info.vertical_datum); +// } else { +// RCLCPP_ERROR_STREAM_THROTTLE( +// this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), +// "Unknown Projector type"); +// } +// return gnss_stat; +// } + +// geometry_msgs::msg::Point GNSSPoser::getPosition(const GNSSStat & gnss_stat) +// { +// geometry_msgs::msg::Point point; +// point.x = gnss_stat.x; +// point.y = gnss_stat.y; +// point.z = gnss_stat.z; +// return point; +// } geometry_msgs::msg::Point GNSSPoser::getMedianPosition( const boost::circular_buffer & position_buffer) From f6a4b3b680bf2f2abfc069d39df79ebdc9a9d8f0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Sep 2023 01:26:46 +0000 Subject: [PATCH 24/42] style(pre-commit): autofix --- .../include/gnss_poser/gnss_poser_core.hpp | 6 ++---- sensing/gnss_poser/src/gnss_poser_core.cpp | 12 +++++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 93d81c8db194c..7d17b9464f38f 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -63,10 +63,8 @@ class GNSSPoser : public rclcpp::Node // GNSSStat convert( // const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, // const MapProjectorInfo::Message & projector_info); - Point getMedianPosition( - const boost::circular_buffer & position_buffer); - Point getAveragePosition( - const boost::circular_buffer & position_buffer); + Point getMedianPosition(const boost::circular_buffer & position_buffer); + Point getAveragePosition(const boost::circular_buffer & position_buffer); geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( const Point & point, const Point & prev_point); diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 15db73fa7cc31..6f8acf785ccce 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -14,9 +14,11 @@ #include "gnss_poser/gnss_poser_core.hpp" -#include -#include #include +#include + +#include + #include #include #include @@ -100,8 +102,8 @@ void GNSSPoser::callbackNavSatFix( gps_point.altitude = nav_sat_fix_msg_ptr->altitude; Point position = geography_utils::project_forward(gps_point, projector_info_); position.z = geography_utils::convert_height( - gps_point.altitude, gps_point.latitude, gps_point.longitude, - "WGS84", projector_info_.vertical_datum); + gps_point.altitude, gps_point.latitude, gps_point.longitude, "WGS84", + projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -209,7 +211,7 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix // const MapProjectorInfo::Message & map_projector_info) // { // GNSSStat gnss_stat; - + // if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { // gnss_stat = NavSatFix2LocalCartesianUTM( // nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(), From a9a59e4183dc23fbbc9e2e5fd3cc461ca1fb90ff Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 10:30:39 +0900 Subject: [PATCH 25/42] remove unnecessary files Signed-off-by: kminoda --- sensing/gnss_poser/CMakeLists.txt | 2 - .../gnss_poser/include/gnss_poser/convert.hpp | 145 ------------------ .../include/gnss_poser/gnss_poser_core.hpp | 6 - .../include/gnss_poser/gnss_stat.hpp | 48 ------ 4 files changed, 201 deletions(-) delete mode 100644 sensing/gnss_poser/include/gnss_poser/convert.hpp delete mode 100644 sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index bd9886d376cad..167f2c51337ce 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -16,9 +16,7 @@ find_library(GeographicLib_LIBRARIES ) set(GNSS_POSER_HEADERS - include/gnss_poser/convert.hpp include/gnss_poser/gnss_poser_core.hpp - include/gnss_poser/gnss_stat.hpp ) ament_auto_add_library(gnss_poser_node SHARED diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp deleted file mode 100644 index 5cd0767f28130..0000000000000 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ /dev/null @@ -1,145 +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. -#ifndef GNSS_POSER__CONVERT_HPP_ -#define GNSS_POSER__CONVERT_HPP_ - -#include "gnss_poser/gnss_stat.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -#include - -namespace gnss_poser -{ -enum class MGRSPrecision { - _10_KILO_METER = 1, - _1_KILO_METER = 2, - _100_METER = 3, - _10_METER = 4, - _1_METER = 5, - _100_MIllI_METER = 6, - _10_MIllI_METER = 7, - _1_MIllI_METER = 8, - _100MICRO_METER = 9, -}; - -GNSSStat NavSatFix2UTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - const std::string & target_vertical_datum) -{ - 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); - utm.z = geography_utils::convert_height( - nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", - target_vertical_datum); - 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, - const geographic_msgs::msg::GeoPoint geo_point_origin, const rclcpp::Logger & logger, - const std::string & target_vertical_datum) -{ - GNSSStat utm_local; - try { - // origin of the local coordinate system in global frame - GNSSStat utm_origin; - GeographicLib::UTMUPS::Forward( - geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone, - utm_origin.east_north_up, utm_origin.x, utm_origin.y); - - utm_origin.z = geography_utils::convert_height( - geo_point_origin.altitude, geo_point_origin.latitude, geo_point_origin.longitude, "WGS84", - target_vertical_datum); - - // 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 = geography_utils::convert_height( - nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, - "WGS84", target_vertical_datum) - - 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, const std::string & vertical_datum) -{ - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, vertical_datum); - const auto mgrs = UTM2MGRS(utm, precision, logger); - return mgrs; -} - -} // namespace gnss_poser - -#endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 7d17b9464f38f..b692131b59148 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -14,9 +14,6 @@ #ifndef GNSS_POSER__GNSS_POSER_CORE_HPP_ #define GNSS_POSER__GNSS_POSER_CORE_HPP_ -// #include "gnss_poser/convert.hpp" -// #include "gnss_poser/gnss_stat.hpp" - #include #include #include @@ -60,9 +57,6 @@ class GNSSPoser : public rclcpp::Node bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - // GNSSStat convert( - // const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - // const MapProjectorInfo::Message & projector_info); Point getMedianPosition(const boost::circular_buffer & position_buffer); Point getAveragePosition(const boost::circular_buffer & position_buffer); geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp deleted file mode 100644 index acd21a83d96e4..0000000000000 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ /dev/null @@ -1,48 +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. -#ifndef GNSS_POSER__GNSS_STAT_HPP_ -#define GNSS_POSER__GNSS_STAT_HPP_ - -#include - -namespace gnss_poser -{ -struct GNSSStat -{ - GNSSStat() - : east_north_up(true), - zone(0), - mgrs_zone(""), - x(0), - y(0), - z(0), - latitude(0), - longitude(0), - altitude(0) - { - } - - bool east_north_up; - int zone; - std::string mgrs_zone; - double x; - double y; - double z; - double latitude; - double longitude; - double altitude; -}; -} // namespace gnss_poser - -#endif // GNSS_POSER__GNSS_STAT_HPP_ From eb57761cfb22a920a067ee82e271998995b94f00 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 10:35:23 +0900 Subject: [PATCH 26/42] remove unnecessary parts Signed-off-by: kminoda --- sensing/gnss_poser/src/gnss_poser_core.cpp | 31 ---------------------- 1 file changed, 31 deletions(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 6f8acf785ccce..f3d7ec6c7ec6d 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -206,37 +206,6 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -// GNSSStat GNSSPoser::convert( -// const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, -// const MapProjectorInfo::Message & map_projector_info) -// { -// GNSSStat gnss_stat; - -// if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { -// gnss_stat = NavSatFix2LocalCartesianUTM( -// nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(), -// map_projector_info.vertical_datum); -// } else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) { -// gnss_stat = NavSatFix2MGRS( -// nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), -// map_projector_info.vertical_datum); -// } else { -// RCLCPP_ERROR_STREAM_THROTTLE( -// this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), -// "Unknown Projector type"); -// } -// return gnss_stat; -// } - -// geometry_msgs::msg::Point GNSSPoser::getPosition(const GNSSStat & gnss_stat) -// { -// geometry_msgs::msg::Point point; -// point.x = gnss_stat.x; -// point.y = gnss_stat.y; -// point.z = gnss_stat.z; -// return point; -// } - geometry_msgs::msg::Point GNSSPoser::getMedianPosition( const boost::circular_buffer & position_buffer) { From ffb17f9cb17e924319641a5a7d3d1a1094314930 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 10:35:39 +0900 Subject: [PATCH 27/42] remove lanelet2 dependency from default_ad_api Signed-off-by: kminoda --- system/default_ad_api/package.xml | 1 - system/default_ad_api/src/vehicle.cpp | 9 --------- system/default_ad_api/src/vehicle.hpp | 6 ------ 3 files changed, 16 deletions(-) diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 9af361739129e..6a9a22f39cdc4 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -25,7 +25,6 @@ component_interface_utils geographic_msgs geography_utils - lanelet2_extension motion_utils nav_msgs rclcpp diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 7f211e106988d..03ae79120a803 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -94,15 +94,6 @@ void VehicleNode::kinematic_state( kinematic_state_msgs_ = msg_ptr; } -Eigen::Vector3d VehicleNode::toBasicPoint3dPt(const geometry_msgs::msg::Point src) -{ - Eigen::Vector3d dst; - dst.x() = src.x; - dst.y() = src.y; - dst.z() = src.z; - return dst; -} - void VehicleNode::acceleration_status( const localization_interface::Acceleration::Message::ConstSharedPtr msg_ptr) { diff --git a/system/default_ad_api/src/vehicle.hpp b/system/default_ad_api/src/vehicle.hpp index 90f8b686206c5..56012152a132b 100644 --- a/system/default_ad_api/src/vehicle.hpp +++ b/system/default_ad_api/src/vehicle.hpp @@ -15,8 +15,6 @@ #ifndef VEHICLE_HPP_ #define VEHICLE_HPP_ -#include "lanelet2_extension/projection/mgrs_projector.hpp" - #include #include #include @@ -27,9 +25,6 @@ #include #include -#include -#include - #include // This file should be included after messages. @@ -83,7 +78,6 @@ class VehicleNode : public rclcpp::Node void publish_kinematics(); void publish_status(); void on_timer(); - Eigen::Vector3d toBasicPoint3dPt(const geometry_msgs::msg::Point src); }; } // namespace default_ad_api From 051517e1edd0493df037a6ca0fd82507e94a78fa Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 10:49:46 +0900 Subject: [PATCH 28/42] remove geography_utils.hpp Signed-off-by: kminoda --- .../geography_utils/geography_utils.hpp | 21 ------------------- 1 file changed, 21 deletions(-) delete mode 100644 common/geography_utils/include/geography_utils/geography_utils.hpp diff --git a/common/geography_utils/include/geography_utils/geography_utils.hpp b/common/geography_utils/include/geography_utils/geography_utils.hpp deleted file mode 100644 index 8dee8d27dbacd..0000000000000 --- a/common/geography_utils/include/geography_utils/geography_utils.hpp +++ /dev/null @@ -1,21 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef GEOGRAPHY_UTILS__GEOGRAPHY_UTILS_HPP_ -#define GEOGRAPHY_UTILS__GEOGRAPHY_UTILS_HPP_ - -#include "geography_utils/height.hpp" -#include "geography_utils/projection.hpp" - -#endif // GEOGRAPHY_UTILS__GEOGRAPHY_UTILS_HPP_ From 2b1eec387401114e4cc6aa20b645f0f0789bbfbe Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 11:57:58 +0900 Subject: [PATCH 29/42] add get_lanelet2_projector and use that in whole system Signed-off-by: kminoda --- common/geography_utils/CMakeLists.txt | 1 + .../geography_utils/lanelet2_projector.hpp | 33 +++++++++ .../src/lanelet2_projector.cpp | 42 +++++++++++ common/geography_utils/src/projection.cpp | 71 ++++++++----------- .../map_loader/lanelet2_map_loader_node.hpp | 3 +- map/map_loader/package.xml | 1 + .../lanelet2_map_loader_node.cpp | 27 ++----- 7 files changed, 114 insertions(+), 64 deletions(-) create mode 100644 common/geography_utils/include/geography_utils/lanelet2_projector.hpp create mode 100644 common/geography_utils/src/lanelet2_projector.cpp diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt index 9c38486f29c93..b705e819c059b 100644 --- a/common/geography_utils/CMakeLists.txt +++ b/common/geography_utils/CMakeLists.txt @@ -15,6 +15,7 @@ find_library(GeographicLib_LIBRARIES NAMES Geographic) ament_auto_add_library(geography_utils SHARED src/height.cpp src/projection.cpp + src/lanelet2_projector.cpp ) target_link_libraries(geography_utils diff --git a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp new file mode 100644 index 0000000000000..5ca65d69233bf --- /dev/null +++ b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#define GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ + +#include +#include +#include +#include + +#include + +namespace geography_utils +{ +using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp new file mode 100644 index 0000000000000..4f7bd9f2c867b --- /dev/null +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -0,0 +1,42 @@ +// 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 +#include + +#include +#include + +namespace geography_utils +{ + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) +{ + if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + lanelet::GPSPoint position{projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + lanelet::Origin origin{position}; + lanelet::projection::UtmProjector projector{origin}; + return std::make_unique(projector); + } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + lanelet::projection::MGRSProjector projector{}; + return std::make_unique(projector); + } else { + const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, and LocalCartesianUTM"; + throw std::invalid_argument(error_msg); + } +} + +} // namespace geography_utils \ No newline at end of file diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index e542d69426387..2839cc3abe166 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -14,10 +14,10 @@ #include #include +#include #include - -#include -#include +// #include +// #include #include #include @@ -38,55 +38,42 @@ Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & projector_info) { - LocalPoint local_point; - if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - lanelet::BasicPoint3d projected_local_point = projector.forward(position); - local_point.x = projected_local_point.x(); - local_point.y = projected_local_point.y(); - local_point.z = projected_local_point.z(); - } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + std::unique_ptr projector = get_lanelet2_projector(projector_info); + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; + + lanelet::BasicPoint3d projected_local_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { const int mgrs_precision = 9; // set precision as 100 micro meter - lanelet::projection::MGRSProjector projector{}; - lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; - lanelet::BasicPoint3d projected_local_point = projector.forward(position, mgrs_precision); - local_point.x = projected_local_point.x(); - local_point.y = projected_local_point.y(); - local_point.z = projected_local_point.z(); + const auto mgrs_projector = dynamic_cast(projector.get()); + projected_local_point = mgrs_projector->forward(position, mgrs_precision); } else { - const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, and LocalCartesianUTM"; - throw std::runtime_error(error_msg); + projected_local_point = projector->forward(position); } + + LocalPoint local_point; + local_point.x = projected_local_point.x(); + local_point.y = projected_local_point.y(); + local_point.z = projected_local_point.z(); + return local_point; } GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & projector_info) { - GeoPoint geo_point; - if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{ - projector_info.map_origin.latitude, projector_info.map_origin.longitude, - projector_info.map_origin.altitude}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - lanelet::GPSPoint projected_gps_point = projector.reverse(to_basic_point_3d_pt(local_point)); - geo_point.latitude = projected_gps_point.lat; - geo_point.longitude = projected_gps_point.lon; - geo_point.altitude = projected_gps_point.ele; - } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { - lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse( - to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); - geo_point.latitude = projected_gps_point.lat; - geo_point.longitude = projected_gps_point.lon; - geo_point.altitude = projected_gps_point.ele; + std::unique_ptr projector = get_lanelet2_projector(projector_info); + + lanelet::GPSPoint projected_gps_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const auto mgrs_projector = dynamic_cast(projector.get()); + projected_gps_point = mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); } else { - const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, and LocalCartesianUTM"; - throw std::runtime_error(error_msg); + projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); } + + GeoPoint geo_point; + geo_point.latitude = projected_gps_point.lat; + geo_point.longitude = projected_gps_point.lon; + geo_point.altitude = projected_gps_point.ele; return geo_point; } diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index d2e9f66ad047a..6e21e9f8ddcb6 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -36,8 +36,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); static lanelet::LaneletMapPtr load_map( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0); + const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); static HADMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index fbc2572d0bc6d..29a2cb7dc8cbb 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -33,6 +33,7 @@ tier4_map_msgs visualization_msgs yaml-cpp + geography_utils ament_cmake_gmock ament_lint_auto diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 328808e04d9e0..d64425bd52750 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -33,6 +33,7 @@ #include "map_loader/lanelet2_map_loader_node.hpp" +#include #include #include #include @@ -69,8 +70,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); // load map from file - const auto map = load_map( - lanelet2_filename, msg->projector_type, msg->map_origin.latitude, msg->map_origin.longitude); + const auto map = load_map(lanelet2_filename, *msg); if (!map) { return; } @@ -88,26 +88,16 @@ void Lanelet2MapLoaderNode::on_map_projector_info( } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon) + const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info) { lanelet::ErrorMessages errors{}; - if (lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) { - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - } else if ( - lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + std::unique_ptr projector = geography_utils::get_lanelet2_projector(projector_info); + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; } - } else if (lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + } else { // Use MGRSProjector as parser lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); @@ -132,9 +122,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( } return map; - } else { - RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); - return nullptr; } for (const auto & error : errors) { From 1596e7ba18c847537a2a1c89ad75bf56bd57fdd7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Sep 2023 02:59:40 +0000 Subject: [PATCH 30/42] style(pre-commit): autofix --- .../include/geography_utils/lanelet2_projector.hpp | 1 + common/geography_utils/src/lanelet2_projector.cpp | 9 +++++---- common/geography_utils/src/projection.cpp | 5 +++-- .../include/map_loader/lanelet2_map_loader_node.hpp | 3 ++- map/map_loader/package.xml | 2 +- .../src/lanelet2_map_loader/lanelet2_map_loader_node.cpp | 8 +++++--- 6 files changed, 17 insertions(+), 11 deletions(-) diff --git a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp index 5ca65d69233bf..8d6cdb317f05e 100644 --- a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp +++ b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp @@ -18,6 +18,7 @@ #include #include #include + #include #include diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp index 4f7bd9f2c867b..d487a5273ce79 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -14,8 +14,8 @@ #include #include - #include + #include namespace geography_utils @@ -24,8 +24,9 @@ namespace geography_utils std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) { if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{projector_info.map_origin.latitude, projector_info.map_origin.longitude, - projector_info.map_origin.altitude}; + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; return std::make_unique(projector); @@ -39,4 +40,4 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf } } -} // namespace geography_utils \ No newline at end of file +} // namespace geography_utils diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index 2839cc3abe166..073f290f3173f 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include -#include #include +#include #include // #include // #include @@ -65,7 +65,8 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & lanelet::GPSPoint projected_gps_point; if (projector_info.projector_type == MapProjectorInfo::MGRS) { const auto mgrs_projector = dynamic_cast(projector.get()); - projected_gps_point = mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); + projected_gps_point = + mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); } else { projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); } diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 6e21e9f8ddcb6..0adc7612e9f61 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -36,7 +36,8 @@ class Lanelet2MapLoaderNode : public rclcpp::Node explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); static lanelet::LaneletMapPtr load_map( - const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); + const std::string & lanelet2_filename, + const tier4_map_msgs::msg::MapProjectorInfo & projector_info); static HADMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 29a2cb7dc8cbb..e663d19d516ac 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,6 +19,7 @@ component_interface_specs component_interface_utils fmt + geography_utils geometry_msgs lanelet2_extension libpcl-all-dev @@ -33,7 +34,6 @@ tier4_map_msgs visualization_msgs yaml-cpp - geography_utils ament_cmake_gmock ament_lint_auto diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index d64425bd52750..259c168edcc5c 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -33,8 +33,8 @@ #include "map_loader/lanelet2_map_loader_node.hpp" -#include #include +#include #include #include #include @@ -88,11 +88,13 @@ void Lanelet2MapLoaderNode::on_map_projector_info( } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( - const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info) + const std::string & lanelet2_filename, + const tier4_map_msgs::msg::MapProjectorInfo & projector_info) { lanelet::ErrorMessages errors{}; if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { - std::unique_ptr projector = geography_utils::get_lanelet2_projector(projector_info); + std::unique_ptr projector = + geography_utils::get_lanelet2_projector(projector_info); const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; From bded3ba5e61ce3b8c0941e8ea85b9aa8677229b2 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 14:14:48 +0900 Subject: [PATCH 31/42] remove unnecessary parts Signed-off-by: kminoda --- common/geography_utils/src/projection.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index 073f290f3173f..f74dcd1d5f707 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -16,8 +16,6 @@ #include #include #include -// #include -// #include #include #include From cad1da5cc9ec2af6797f0745e2934194da4a2654 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 14:17:41 +0900 Subject: [PATCH 32/42] revert using Point Signed-off-by: kminoda --- sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp | 7 +++---- sensing/gnss_poser/src/gnss_poser_core.cpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index b692131b59148..fb7e98f40b0ab 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -48,7 +48,6 @@ class GNSSPoser : public rclcpp::Node private: using MapProjectorInfo = map_interface::MapProjectorInfo; - using Point = geometry_msgs::msg::Point; void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); @@ -57,11 +56,11 @@ class GNSSPoser : public rclcpp::Node bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - Point getMedianPosition(const boost::circular_buffer & position_buffer); - Point getAveragePosition(const boost::circular_buffer & position_buffer); + geometry_msgs::msg::Point getMedianPosition(const boost::circular_buffer & position_buffer); + geometry_msgs::msg::Point getAveragePosition(const boost::circular_buffer & position_buffer); geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( - const Point & point, const Point & prev_point); + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); bool getTransform( const std::string & target_frame, const std::string & source_frame, diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index f3d7ec6c7ec6d..18ebe915e7d4c 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -100,7 +100,7 @@ void GNSSPoser::callbackNavSatFix( gps_point.latitude = nav_sat_fix_msg_ptr->latitude; gps_point.longitude = nav_sat_fix_msg_ptr->longitude; gps_point.altitude = nav_sat_fix_msg_ptr->altitude; - Point position = geography_utils::project_forward(gps_point, projector_info_); + geometry_msgs::msg::Point position = geography_utils::project_forward(gps_point, projector_info_); position.z = geography_utils::convert_height( gps_point.altitude, gps_point.latitude, gps_point.longitude, "WGS84", projector_info_.vertical_datum); From 8b1b03b791525495776ee9f1f1c117f481bb7761 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Sep 2023 05:21:36 +0000 Subject: [PATCH 33/42] style(pre-commit): autofix --- sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index fb7e98f40b0ab..2118d33bc4b30 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -56,8 +56,10 @@ class GNSSPoser : public rclcpp::Node bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - geometry_msgs::msg::Point getMedianPosition(const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Point getAveragePosition(const boost::circular_buffer & position_buffer); + geometry_msgs::msg::Point getMedianPosition( + const boost::circular_buffer & position_buffer); + geometry_msgs::msg::Point getAveragePosition( + const boost::circular_buffer & position_buffer); geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); From 34b52352e5c7a0cc5b8b3ac0bcbac244e1158120 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 1 Sep 2023 23:00:48 +0900 Subject: [PATCH 34/42] remove unnecessary files Signed-off-by: kminoda --- common/geography_utils/include/geography_utils/height.hpp | 3 --- .../include/geography_utils/lanelet2_projector.hpp | 2 -- common/geography_utils/src/projection.cpp | 5 ----- 3 files changed, 10 deletions(-) diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp index 1921e79699970..8bcda41f84cda 100644 --- a/common/geography_utils/include/geography_utils/height.hpp +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -15,10 +15,7 @@ #ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_ #define GEOGRAPHY_UTILS__HEIGHT_HPP_ -#include -#include #include -#include namespace geography_utils { diff --git a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp index 8d6cdb317f05e..739abe7019023 100644 --- a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp +++ b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp @@ -15,8 +15,6 @@ #ifndef GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ #define GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ -#include -#include #include #include diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index f74dcd1d5f707..53b836aaa1375 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -17,11 +17,6 @@ #include #include -#include -#include -#include -#include - namespace geography_utils { From 4fb44b6c38b15aa1c9419077f02fdf2ffc069023 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 4 Sep 2023 15:04:45 +0900 Subject: [PATCH 35/42] update test Signed-off-by: kminoda --- common/geography_utils/package.xml | 1 + common/geography_utils/src/projection.cpp | 15 ++ .../test/test_geography_utils.cpp | 26 +++ common/geography_utils/test/test_height.cpp | 6 - .../geography_utils/test/test_projection.cpp | 153 ++++++++++++++++++ 5 files changed, 195 insertions(+), 6 deletions(-) create mode 100644 common/geography_utils/test/test_geography_utils.cpp create mode 100644 common/geography_utils/test/test_projection.cpp diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml index 18ecadc513f36..c50715de2695d 100644 --- a/common/geography_utils/package.xml +++ b/common/geography_utils/package.xml @@ -12,6 +12,7 @@ geographiclib geometry_msgs + geographic_msgs lanelet2_extension lanelet2_io tier4_map_msgs diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index 53b836aaa1375..7814765ab9a7e 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -38,9 +38,17 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr if (projector_info.projector_type == MapProjectorInfo::MGRS) { const int mgrs_precision = 9; // set precision as 100 micro meter const auto mgrs_projector = dynamic_cast(projector.get()); + + // project x and y using projector + // note that the altitude is ignored in MGRS projection conventionally projected_local_point = mgrs_projector->forward(position, mgrs_precision); } else { + // project x and y using projector projected_local_point = projector->forward(position); + + // correct z based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo point + projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; } LocalPoint local_point; @@ -58,10 +66,17 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & lanelet::GPSPoint projected_gps_point; if (projector_info.projector_type == MapProjectorInfo::MGRS) { const auto mgrs_projector = dynamic_cast(projector.get()); + // project latitude and longitude using projector + // note that the z is ignored in MGRS projection conventionally projected_gps_point = mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); } else { + // project latitude and longitude using projector projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); + + // correct altitude based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo point + projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; } GeoPoint geo_point; diff --git a/common/geography_utils/test/test_geography_utils.cpp b/common/geography_utils/test/test_geography_utils.cpp new file mode 100644 index 0000000000000..caf57ec427b50 --- /dev/null +++ b/common/geography_utils/test/test_geography_utils.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "geography_utils/height.hpp" +#include "geography_utils/projection.hpp" +#include "geography_utils/lanelet2_projector.hpp" + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index 3d8a4c9c203fe..aadab07648f4e 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -84,9 +84,3 @@ TEST(Tier4GeographyUtils, InvalidTargetDatum) geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), std::invalid_argument); } - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/geography_utils/test/test_projection.cpp b/common/geography_utils/test/test_projection.cpp new file mode 100644 index 0000000000000..3742d10dd3b03 --- /dev/null +++ b/common/geography_utils/test/test_projection.cpp @@ -0,0 +1,153 @@ +// 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 + +#include +#include +#include + +TEST(Tier4GeographyUtilsProjection, ProjectForwardToMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_point = geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(Tier4GeographyUtilsProjection, ProjectReverseFromMGRS) +{ + // source point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // target point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geographic_msgs::msg::GeoPoint converted_point = geography_utils::project_reverse(local_point, projector_info); + + EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(Tier4GeographyUtilsProjection, ProjectForwardAndReverseMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_local_point = geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(Tier4GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62406; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 0.0; + local_point.y = -22.18; + local_point.z = 20.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.62426; + projector_info.map_origin.longitude = 139.74252; + projector_info.map_origin.altitude = -10.0; + + // conversion + const geometry_msgs::msg::Point converted_point = geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(Tier4GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.0; + projector_info.map_origin.longitude = 139.0; + projector_info.map_origin.altitude = 0.0; + + // conversion + const geometry_msgs::msg::Point converted_local_point = geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} From 7dd3fc961904894033cbf07e90f9649293e816a1 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 4 Sep 2023 15:06:36 +0900 Subject: [PATCH 36/42] update comment Signed-off-by: kminoda --- common/geography_utils/src/projection.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index 7814765ab9a7e..61afb17b90057 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -44,6 +44,7 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr projected_local_point = mgrs_projector->forward(position, mgrs_precision); } else { // project x and y using projector + // note that the original projector such as UTM projector does not compensate for the altitude offset projected_local_point = projector->forward(position); // correct z based on the map origin @@ -72,6 +73,7 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); } else { // project latitude and longitude using projector + // note that the original projector such as UTM projector does not compensate for the altitude offset projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); // correct altitude based on the map origin From f6ec88ee9a7ddf3a05b819ee6ce6fbdc942f0817 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 4 Sep 2023 06:06:39 +0000 Subject: [PATCH 37/42] style(pre-commit): autofix --- common/geography_utils/package.xml | 2 +- common/geography_utils/src/projection.cpp | 10 +++++---- .../test/test_geography_utils.cpp | 2 +- .../geography_utils/test/test_projection.cpp | 22 +++++++++++++------ 4 files changed, 23 insertions(+), 13 deletions(-) diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml index c50715de2695d..414364e36ad29 100644 --- a/common/geography_utils/package.xml +++ b/common/geography_utils/package.xml @@ -10,9 +10,9 @@ ament_cmake_auto autoware_cmake + geographic_msgs geographiclib geometry_msgs - geographic_msgs lanelet2_extension lanelet2_io tier4_map_msgs diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index 61afb17b90057..4da80fcb012cb 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -40,7 +40,7 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr const auto mgrs_projector = dynamic_cast(projector.get()); // project x and y using projector - // note that the altitude is ignored in MGRS projection conventionally + // note that the altitude is ignored in MGRS projection conventionally projected_local_point = mgrs_projector->forward(position, mgrs_precision); } else { // project x and y using projector @@ -48,7 +48,8 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr projected_local_point = projector->forward(position); // correct z based on the map origin - // note that the converted altitude in local point is in the same vertical datum as the geo point + // note that the converted altitude in local point is in the same vertical datum as the geo + // point projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; } @@ -68,7 +69,7 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & if (projector_info.projector_type == MapProjectorInfo::MGRS) { const auto mgrs_projector = dynamic_cast(projector.get()); // project latitude and longitude using projector - // note that the z is ignored in MGRS projection conventionally + // note that the z is ignored in MGRS projection conventionally projected_gps_point = mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); } else { @@ -77,7 +78,8 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); // correct altitude based on the map origin - // note that the converted altitude in local point is in the same vertical datum as the geo point + // note that the converted altitude in local point is in the same vertical datum as the geo + // point projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; } diff --git a/common/geography_utils/test/test_geography_utils.cpp b/common/geography_utils/test/test_geography_utils.cpp index caf57ec427b50..480e17b8f49a4 100644 --- a/common/geography_utils/test/test_geography_utils.cpp +++ b/common/geography_utils/test/test_geography_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "geography_utils/height.hpp" -#include "geography_utils/projection.hpp" #include "geography_utils/lanelet2_projector.hpp" +#include "geography_utils/projection.hpp" #include diff --git a/common/geography_utils/test/test_projection.cpp b/common/geography_utils/test/test_projection.cpp index 3742d10dd3b03..bacfc98f1baa0 100644 --- a/common/geography_utils/test/test_projection.cpp +++ b/common/geography_utils/test/test_projection.cpp @@ -15,6 +15,7 @@ #include #include + #include #include @@ -39,7 +40,8 @@ TEST(Tier4GeographyUtilsProjection, ProjectForwardToMGRS) projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; // conversion - const geometry_msgs::msg::Point converted_point = geography_utils::project_forward(geo_point, projector_info); + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); EXPECT_NEAR(converted_point.x, local_point.x, 1.0); EXPECT_NEAR(converted_point.y, local_point.y, 1.0); @@ -67,7 +69,8 @@ TEST(Tier4GeographyUtilsProjection, ProjectReverseFromMGRS) projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; // conversion - const geographic_msgs::msg::GeoPoint converted_point = geography_utils::project_reverse(local_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_point = + geography_utils::project_reverse(local_point, projector_info); EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); @@ -89,8 +92,10 @@ TEST(Tier4GeographyUtilsProjection, ProjectForwardAndReverseMGRS) projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; // conversion - const geometry_msgs::msg::Point converted_local_point = geography_utils::project_forward(geo_point, projector_info); - const geographic_msgs::msg::GeoPoint converted_geo_point = geography_utils::project_reverse(converted_local_point, projector_info); + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); @@ -120,7 +125,8 @@ TEST(Tier4GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) projector_info.map_origin.altitude = -10.0; // conversion - const geometry_msgs::msg::Point converted_point = geography_utils::project_forward(geo_point, projector_info); + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); EXPECT_NEAR(converted_point.x, local_point.x, 1.0); EXPECT_NEAR(converted_point.y, local_point.y, 1.0); @@ -144,8 +150,10 @@ TEST(Tier4GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOri projector_info.map_origin.altitude = 0.0; // conversion - const geometry_msgs::msg::Point converted_local_point = geography_utils::project_forward(geo_point, projector_info); - const geographic_msgs::msg::GeoPoint converted_geo_point = geography_utils::project_reverse(converted_local_point, projector_info); + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); From 21e0765796e22e034f40c68194c8582f9df157f2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 4 Sep 2023 06:08:25 +0000 Subject: [PATCH 38/42] style(pre-commit): autofix --- common/geography_utils/src/projection.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index 4da80fcb012cb..fd930d4c05e4f 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -44,7 +44,8 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr projected_local_point = mgrs_projector->forward(position, mgrs_precision); } else { // project x and y using projector - // note that the original projector such as UTM projector does not compensate for the altitude offset + // note that the original projector such as UTM projector does not compensate for the altitude + // offset projected_local_point = projector->forward(position); // correct z based on the map origin @@ -74,7 +75,8 @@ GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); } else { // project latitude and longitude using projector - // note that the original projector such as UTM projector does not compensate for the altitude offset + // note that the original projector such as UTM projector does not compensate for the altitude + // offset projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); // correct altitude based on the map origin From 095e27d8f0e996ed1598f0dfd3782c58e970d60f Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 4 Sep 2023 15:23:41 +0900 Subject: [PATCH 39/42] use constant string instead Signed-off-by: kminoda --- sensing/gnss_poser/src/gnss_poser_core.cpp | 2 +- system/default_ad_api/src/vehicle.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 18ebe915e7d4c..5c0e4e6e5e06a 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -102,7 +102,7 @@ void GNSSPoser::callbackNavSatFix( gps_point.altitude = nav_sat_fix_msg_ptr->altitude; geometry_msgs::msg::Point position = geography_utils::project_forward(gps_point, projector_info_); position.z = geography_utils::convert_height( - gps_point.altitude, gps_point.latitude, gps_point.longitude, "WGS84", + gps_point.altitude, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{}; diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 03ae79120a803..e25933e6ca2ad 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -151,7 +151,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.longitude; vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, - map_projector_info_->vertical_datum, "WGS84"); + map_projector_info_->vertical_datum, MapProjectorInfo::WGS84); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = From efbe26ef6ca9c3a283a7e6268efbcdf6777bb300 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 4 Sep 2023 15:41:18 +0900 Subject: [PATCH 40/42] use reference Signed-off-by: kminoda --- common/geography_utils/include/geography_utils/projection.hpp | 4 ++-- common/geography_utils/src/projection.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/geography_utils/include/geography_utils/projection.hpp index 6b74e06532b49..be616d854a466 100644 --- a/common/geography_utils/include/geography_utils/projection.hpp +++ b/common/geography_utils/include/geography_utils/projection.hpp @@ -25,8 +25,8 @@ using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; using GeoPoint = geographic_msgs::msg::GeoPoint; using LocalPoint = geometry_msgs::msg::Point; -LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & projector_info); -GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & projector_info); +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info); +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info); } // namespace geography_utils diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp index fd930d4c05e4f..ac9ce19052702 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/geography_utils/src/projection.cpp @@ -29,7 +29,7 @@ Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) return dst; } -LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & projector_info) +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) { std::unique_ptr projector = get_lanelet2_projector(projector_info); lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; @@ -62,7 +62,7 @@ LocalPoint project_forward(const GeoPoint geo_point, const MapProjectorInfo & pr return local_point; } -GeoPoint project_reverse(const LocalPoint local_point, const MapProjectorInfo & projector_info) +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info) { std::unique_ptr projector = get_lanelet2_projector(projector_info); From 4cedde7f0b66f166213cdb97e5cb753c00810e50 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 4 Sep 2023 15:45:46 +0900 Subject: [PATCH 41/42] fix test name Signed-off-by: kminoda --- common/geography_utils/test/test_height.cpp | 10 +++++----- common/geography_utils/test/test_projection.cpp | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index aadab07648f4e..15297089ee131 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -20,7 +20,7 @@ #include // Test case to verify if same source and target datums return original height -TEST(Tier4GeographyUtils, SameSourceTargetDatum) +TEST(GeographyUtils, SameSourceTargetDatum) { const double height = 10.0; const double latitude = 35.0; @@ -34,7 +34,7 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum) } // Test case to verify valid source and target datums -TEST(Tier4GeographyUtils, ValidSourceTargetDatum) +TEST(GeographyUtils, ValidSourceTargetDatum) { // Calculated with // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html @@ -50,7 +50,7 @@ TEST(Tier4GeographyUtils, ValidSourceTargetDatum) } // Test case to verify invalid source and target datums -TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) +TEST(GeographyUtils, InvalidSourceTargetDatum) { const double height = 10.0; const double latitude = 35.0; @@ -62,7 +62,7 @@ TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) } // Test case to verify invalid source datums -TEST(Tier4GeographyUtils, InvalidSourceDatum) +TEST(GeographyUtils, InvalidSourceDatum) { const double height = 10.0; const double latitude = 35.0; @@ -74,7 +74,7 @@ TEST(Tier4GeographyUtils, InvalidSourceDatum) } // Test case to verify invalid target datums -TEST(Tier4GeographyUtils, InvalidTargetDatum) +TEST(GeographyUtils, InvalidTargetDatum) { const double height = 10.0; const double latitude = 35.0; diff --git a/common/geography_utils/test/test_projection.cpp b/common/geography_utils/test/test_projection.cpp index bacfc98f1baa0..74ffb616cde6f 100644 --- a/common/geography_utils/test/test_projection.cpp +++ b/common/geography_utils/test/test_projection.cpp @@ -19,7 +19,7 @@ #include #include -TEST(Tier4GeographyUtilsProjection, ProjectForwardToMGRS) +TEST(GeographyUtilsProjection, ProjectForwardToMGRS) { // source point geographic_msgs::msg::GeoPoint geo_point; @@ -48,7 +48,7 @@ TEST(Tier4GeographyUtilsProjection, ProjectForwardToMGRS) EXPECT_NEAR(converted_point.z, local_point.z, 1.0); } -TEST(Tier4GeographyUtilsProjection, ProjectReverseFromMGRS) +TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) { // source point geometry_msgs::msg::Point local_point; @@ -77,7 +77,7 @@ TEST(Tier4GeographyUtilsProjection, ProjectReverseFromMGRS) EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); } -TEST(Tier4GeographyUtilsProjection, ProjectForwardAndReverseMGRS) +TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) { // source point geographic_msgs::msg::GeoPoint geo_point; @@ -102,7 +102,7 @@ TEST(Tier4GeographyUtilsProjection, ProjectForwardAndReverseMGRS) EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); } -TEST(Tier4GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) +TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) { // source point geographic_msgs::msg::GeoPoint geo_point; @@ -133,7 +133,7 @@ TEST(Tier4GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) EXPECT_NEAR(converted_point.z, local_point.z, 1.0); } -TEST(Tier4GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) +TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) { // source point geographic_msgs::msg::GeoPoint geo_point; From 34273a8bb8a0bf29fe82c0cdaa4f8148823d8b95 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 4 Sep 2023 18:24:19 +0900 Subject: [PATCH 42/42] fix bug Signed-off-by: kminoda --- sensing/gnss_poser/src/gnss_poser_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 5c0e4e6e5e06a..1ae8bce2ed7f2 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -102,7 +102,7 @@ void GNSSPoser::callbackNavSatFix( gps_point.altitude = nav_sat_fix_msg_ptr->altitude; geometry_msgs::msg::Point position = geography_utils::project_forward(gps_point, projector_info_); position.z = geography_utils::convert_height( - gps_point.altitude, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, + position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{};