From 1f0be11f0268eff33a2785ab554bd4cf532414f0 Mon Sep 17 00:00:00 2001 From: Ata Parlar <44242212+ataparlar@users.noreply.github.com> Date: Wed, 20 Jul 2022 14:28:59 +0300 Subject: [PATCH] fix(gnss_poser): added local coordinate system in UTM to gnss_poser (#1227) Signed-off-by: Ata Parlar --- sensing/gnss_poser/README.md | 29 +++++++------- .../config/set_local_origin.param.yaml | 6 +-- .../gnss_poser/include/gnss_poser/convert.hpp | 38 +++++++++++++++++-- .../include/gnss_poser/gnss_stat.hpp | 3 +- .../gnss_poser/launch/gnss_poser.launch.xml | 2 +- sensing/gnss_poser/src/gnss_poser_core.cpp | 8 +++- 6 files changed, 60 insertions(+), 26 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 786d900b3c2b3..69925cc43d0f3 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -10,10 +10,10 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Input -| Name | Type | Description | -| ---------------- | ----------------------------- | ----------------------------------------------------------------------------------------------- | -| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | -| `~/input/navpvt` | `ublox_msgs::msg::NavPVT` | position, velocity and time solution (You can see detail description in reference document [1]) | +| Name | Type | Description | +| ---------------- | ----------------------------- | --------------------------------------------------------------------------------------------------------------- | +| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | +| `~/input/navpvt` | `ublox_msgs::msg::NavPVT` | position, velocity and time solution. [click here for more details](https://github.com/KumarRobotics/ublox.git) | ### Output @@ -27,14 +27,15 @@ 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 d | -| `gnss_frame` | string | "gnss" | frame id | -| `gnss_base_frame` | string | "gnss_base_link" | frame id | -| `map_frame` | string | "map" | frame id | -| `use_ublox_receiver` | bool | false | flag to use ublox receiver | -| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems (See, reference document [2]) | +| 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; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | +| `use_ublox_receiver` | bool | false | flag to use ublox receiver | +| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. [click here for more details](https://www.gsi.go.jp/LAW/heimencho.html) | ## Assumptions / Known limits @@ -44,8 +45,4 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ## (Optional) References/External links -[1] - -[2] - ## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml index 878b8fa2b944e..e931e5adceb68 100644 --- a/sensing/gnss_poser/config/set_local_origin.param.yaml +++ b/sensing/gnss_poser/config/set_local_origin.param.yaml @@ -2,10 +2,10 @@ ros__parameters: # Latitude of local origin - latitude: 40.816617984672746 + latitude: 40.81187906 # Longitude of local origin - longitude: 29.360491808334285 + longitude: 29.35810110 # Altitude of local origin - altitude: 52.251157145314075 + altitude: 47.62 diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 517b871b52f0b..ae74083048c64 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -57,12 +57,12 @@ double EllipsoidHeight2OrthometricHeight( } return OrthometricHeight; } -GNSSStat NavSatFix2LocalCartesian( +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; - local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN; + local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN_WGS84; try { GeographicLib::LocalCartesian localCartesian_origin( @@ -99,7 +99,39 @@ GNSSStat NavSatFix2UTM( } return utm; } - +GNSSStat NavSatFix2LocalCartesianUTM( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger) +{ + GNSSStat utm_local; + utm_local.coordinate_system = CoordinateSystem::UTM; + try { + // origin of the local coordinate system in global frame + GNSSStat utm_origin; + utm_origin.coordinate_system = CoordinateSystem::UTM; + GeographicLib::UTMUPS::Forward( + nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, + utm_origin.northup, utm_origin.x, utm_origin.y); + utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger); + // 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.northup, + 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 = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - 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) { diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index 20074be5ec48c..0dc82102afc82 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -20,7 +20,8 @@ enum class CoordinateSystem { UTM = 0, MGRS = 1, PLANE = 2, - LOCAL_CARTESIAN = 3, + LOCAL_CARTESIAN_WGS84 = 3, + 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 1e2f56597020e..5be5d6ae44b52 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 b4c1590e264a7..d78b1cc8bd02f 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -184,12 +184,16 @@ GNSSStat GNSSPoser::convert( GNSSStat gnss_stat; if (coordinate_system == CoordinateSystem::UTM) { gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, 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()); } else if (coordinate_system == CoordinateSystem::MGRS) { gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger()); } 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) { - gnss_stat = NavSatFix2LocalCartesian(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); + } 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(),