From bb702a68d97b218004c7e871afa4f0de42dd7f3c Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 28 Aug 2023 14:54:12 +0900 Subject: [PATCH] feat(gnss_poser): remove local cartesian wgs84 coordinate (#4726) * feat(gnss_poser): remove local cartesian wgs84 coordinate Signed-off-by: kminoda * update readme Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/gnss_poser/README.md | 16 +++++++------- .../gnss_poser/include/gnss_poser/convert.hpp | 22 ------------------- .../include/gnss_poser/gnss_stat.hpp | 2 +- .../gnss_poser/launch/gnss_poser.launch.xml | 2 +- sensing/gnss_poser/src/gnss_poser_core.cpp | 3 --- 5 files changed, 10 insertions(+), 35 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 6eef2cc243ef5..3487e9515c48a 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -27,14 +27,14 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------- | -| `base_frame` | string | "base_link" | frame id | -| `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, 3: WGS84 Local Coordinate System, 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. | +| Name | Type | Default Value | Description | +| ---------------------- | ------ | ---------------- | ------------------------------------------------------------------------------------------------------ | +| `base_frame` | string | "base_link" | frame id | +| `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/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 5c1f621626aa2..74c9734833232 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -17,7 +17,6 @@ #include "gnss_poser/gnss_stat.hpp" #include -#include #include #include #include @@ -56,27 +55,6 @@ double EllipsoidHeight2OrthometricHeight( } return OrthometricHeight; } -GNSSStat NavSatFix2LocalCartesianWGS84( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) -{ - GNSSStat local_cartesian; - - try { - GeographicLib::LocalCartesian localCartesian_origin( - nav_sat_fix_origin_.latitude, nav_sat_fix_origin_.longitude, nav_sat_fix_origin_.altitude); - localCartesian_origin.Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, - local_cartesian.x, local_cartesian.y, local_cartesian.z); - - local_cartesian.latitude = nav_sat_fix_msg.latitude; - local_cartesian.longitude = nav_sat_fix_msg.longitude; - local_cartesian.altitude = nav_sat_fix_msg.altitude; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert NavSatFix to LocalCartesian" << err.what()); - } - return local_cartesian; -} GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, int height_system) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index fa17bf0e6e232..d33bd022c3714 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -18,7 +18,7 @@ namespace gnss_poser { -enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_WGS84 = 3, LOCAL_CARTESIAN_UTM = 4 }; +enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_UTM = 4 }; struct GNSSStat { diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 3883a492358d6..8a0ffb8cab8c3 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -13,7 +13,7 @@ - + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 7d244ce188eee..4f0723944913b 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -196,9 +196,6 @@ GNSSStat GNSSPoser::convert( } 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::LOCAL_CARTESIAN_WGS84) { - gnss_stat = - NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); } else { RCLCPP_ERROR_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),