From 4f9822d8073c4f08e0254abf87df82ebc08568d3 Mon Sep 17 00:00:00 2001 From: Ata Parlar Date: Mon, 4 Jul 2022 17:57:08 +0300 Subject: [PATCH 01/14] fix(gnss_poser): UTM local coordinate system is added. #1227 Signed-off-by: Ata Parlar --- sensing/gnss_poser/README.md | 1 + .../config/set_local_origin.param.yaml | 6 ++-- .../gnss_poser/include/gnss_poser/convert.hpp | 35 +++++++++++++++++++ .../include/gnss_poser/gnss_stat.hpp | 3 +- .../gnss_poser/launch/gnss_poser.launch.xml | 2 +- sensing/gnss_poser/src/gnss_poser_core.cpp | 4 ++- 6 files changed, 45 insertions(+), 6 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 786d900b3c2b3..248e1a957ea4c 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -33,6 +33,7 @@ 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" | id number of wanted coordinate system, 4 is a local coordinate system in UTM | | `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]) | 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..35be92dc9368f 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -99,6 +99,41 @@ GNSSStat NavSatFix2UTM( } return utm; } +bool is_utm_origin_initialized = false; +GNSSStat utm_origin; +GNSSStat utm_local; +GNSSStat NavSatFix2UTMLocal( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) +{ + utm_origin.coordinate_system = CoordinateSystem::UTM; + utm_local.coordinate_system = CoordinateSystem::UTM; + double global_x; + double global_y; + try { + if (is_utm_origin_initialized == 0) { + 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_msg, logger); + is_utm_origin_initialized = true; + } else { + 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; + 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..98dda53aa6866 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -184,11 +184,13 @@ 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 = NavSatFix2UTMLocal(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) { + } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) { gnss_stat = NavSatFix2LocalCartesian(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); } else { RCLCPP_ERROR_STREAM_THROTTLE( From af566631c2ccfabec62c834c00ac804de48a53f9 Mon Sep 17 00:00:00 2001 From: Ata Parlar Date: Mon, 4 Jul 2022 18:18:50 +0300 Subject: [PATCH 02/14] fix(gnss_poser): UTM local coordinate system is added. UTM default #1227 Signed-off-by: Ata Parlar --- sensing/gnss_poser/launch/gnss_poser.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 5be5d6ae44b52..1666666b164ad 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -13,7 +13,7 @@ - + From 698c3d2cfc3c25ea589a78983afe7acfb35dc155 Mon Sep 17 00:00:00 2001 From: Ata Parlar Date: Mon, 4 Jul 2022 19:22:47 +0300 Subject: [PATCH 03/14] fix(gnss_poser): Changed the function names to correct ones. Signed-off-by: Ata Parlar --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 6 +++--- sensing/gnss_poser/src/gnss_poser_core.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 35be92dc9368f..90ae8725e443a 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( @@ -102,7 +102,7 @@ GNSSStat NavSatFix2UTM( bool is_utm_origin_initialized = false; GNSSStat utm_origin; GNSSStat utm_local; -GNSSStat NavSatFix2UTMLocal( +GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) { diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 98dda53aa6866..a2ddcdf9a01d3 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -185,13 +185,13 @@ GNSSStat GNSSPoser::convert( 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 = NavSatFix2UTMLocal(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); + 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_WGS84) { - gnss_stat = NavSatFix2LocalCartesian(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); + 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(), From 4d8bcea9568378710095c9233e02b0746e7fd288 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 4 Jul 2022 16:27:37 +0000 Subject: [PATCH 04/14] ci(pre-commit): autofix --- sensing/gnss_poser/src/gnss_poser_core.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index a2ddcdf9a01d3..d78b1cc8bd02f 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -185,13 +185,15 @@ GNSSStat GNSSPoser::convert( 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()); + 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_WGS84) { - gnss_stat = NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); + 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(), From 1dc5f8bb244abb0e74f62f040a59687d1700cb91 Mon Sep 17 00:00:00 2001 From: Ata Parlar Date: Thu, 7 Jul 2022 12:11:36 +0300 Subject: [PATCH 05/14] removed unnecessary -else- and changed readme for coordinate systems Signed-off-by: Ata Parlar --- sensing/gnss_poser/README.md | 18 +++++----- .../gnss_poser/include/gnss_poser/convert.hpp | 34 +++++++++---------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 248e1a957ea4c..f77a50a9607fe 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -27,15 +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 | -| `coordinate_system` | int | "4" | id number of wanted coordinate system, 4 is a local coordinate system in UTM | -| `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 d | +| `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" | id number of wanted coordinate system;
1: UTM
2: MGRS
3: Plane
4: WGS84 Local Coordinate System
5: 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 (See, reference document [2]) | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 90ae8725e443a..0b05fe16d0ce1 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -99,7 +99,8 @@ GNSSStat NavSatFix2UTM( } return utm; } -bool is_utm_origin_initialized = false; + +bool utm_origin_needs_init = true; GNSSStat utm_origin; GNSSStat utm_local; GNSSStat NavSatFix2LocalCartesianUTM( @@ -111,23 +112,22 @@ GNSSStat NavSatFix2LocalCartesianUTM( double global_x; double global_y; try { - if (is_utm_origin_initialized == 0) { - 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_msg, logger); - is_utm_origin_initialized = true; - } else { - 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; - 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; + if (utm_origin_needs_init) { + 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_msg, logger); + utm_origin_needs_init = false; } + 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; + 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()); From 467c02877c0cceea43da7159ec36029036a435f4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 7 Jul 2022 09:13:39 +0000 Subject: [PATCH 06/14] ci(pre-commit): autofix --- sensing/gnss_poser/README.md | 2 +- sensing/gnss_poser/include/gnss_poser/convert.hpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index f77a50a9607fe..2c2c4d731d568 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -28,7 +28,7 @@ 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 | diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 0b05fe16d0ce1..e312aecd37148 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -113,15 +113,15 @@ GNSSStat NavSatFix2LocalCartesianUTM( double global_y; try { if (utm_origin_needs_init) { - 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_msg, logger); - utm_origin_needs_init = false; + 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_msg, logger); + utm_origin_needs_init = false; } GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, utm_origin.northup, - global_x, global_y); + 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; From cf94c1adc6a78462d4c16f8a75cd2e067e77675b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Tue, 19 Jul 2022 15:17:53 +0300 Subject: [PATCH 07/14] update readme MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- sensing/gnss_poser/README.md | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 2c2c4d731d568..ce397b12bf747 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -10,15 +10,15 @@ 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 | Name | Type | Description | -| ------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | +|--------------------------|-------------------------------------------------|----------------------------------------------------------------| | `~/output/pose` | `geometry_msgs::msg::PoseStamped` | vehicle pose calculated from gnss sensing data | | `~/output/gnss_pose_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | vehicle pose with covariance calculated from gnss sensing data | | `~/output/gnss_fixed` | `tier4_debug_msgs::msg::BoolStamped` | gnss fix status | @@ -27,15 +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 | -| `coordinate_system` | int | "4" | id number of wanted coordinate system;
1: UTM
2: MGRS
3: Plane
4: WGS84 Local Coordinate System
5: 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 (See, reference document [2]) | +| 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 | +| `coordinate_system` | int | "4" | coordinate system enumeration; 1: UTM, 2: MGRS, 3: Plane, 4: WGS84 Local Coordinate System, 5: 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 @@ -45,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 From 032269402e46fd33b9f45948594c598c055a8ce4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 19 Jul 2022 12:20:37 +0000 Subject: [PATCH 08/14] ci(pre-commit): autofix --- sensing/gnss_poser/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index ce397b12bf747..c1ea090ab1ca2 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -11,14 +11,14 @@ 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. [click here for more details](https://github.com/KumarRobotics/ublox.git) | ### Output | Name | Type | Description | -|--------------------------|-------------------------------------------------|----------------------------------------------------------------| +| ------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | | `~/output/pose` | `geometry_msgs::msg::PoseStamped` | vehicle pose calculated from gnss sensing data | | `~/output/gnss_pose_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | vehicle pose with covariance calculated from gnss sensing data | | `~/output/gnss_fixed` | `tier4_debug_msgs::msg::BoolStamped` | gnss fix status | @@ -28,7 +28,7 @@ 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 | From be31b37f1187afc80cb5c76547acfd1568085b34 Mon Sep 17 00:00:00 2001 From: Ata Parlar Date: Tue, 19 Jul 2022 20:34:41 +0300 Subject: [PATCH 09/14] fix(gnss_poser): better conversation function for local_cartesian_utm Signed-off-by: Ata Parlar --- .../gnss_poser/include/gnss_poser/convert.hpp | 61 +++++++++---------- .../gnss_poser/launch/gnss_poser.launch.xml | 2 +- 2 files changed, 30 insertions(+), 33 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index e312aecd37148..848f2231b735b 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -99,42 +99,39 @@ GNSSStat NavSatFix2UTM( } return utm; } - -bool utm_origin_needs_init = true; -GNSSStat utm_origin; -GNSSStat utm_local; GNSSStat NavSatFix2LocalCartesianUTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger) { - utm_origin.coordinate_system = CoordinateSystem::UTM; - utm_local.coordinate_system = CoordinateSystem::UTM; - double global_x; - double global_y; - try { - if (utm_origin_needs_init) { - 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_msg, logger); - utm_origin_needs_init = false; + GNSSStat utm_origin; + GNSSStat utm_local; + utm_origin.coordinate_system = CoordinateSystem::UTM; + utm_local.coordinate_system = CoordinateSystem::UTM; + double global_x; + double global_y; + try { + // origin of the local coordinate system in global + 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 + 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()); } - 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; - 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; + return utm_local; } - GNSSStat UTM2MGRS( const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) { diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 1666666b164ad..5be5d6ae44b52 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -13,7 +13,7 @@ - + From fe8842938c2f07966df2002da48e1ad09ec5e7e0 Mon Sep 17 00:00:00 2001 From: Ata Parlar Date: Wed, 20 Jul 2022 11:31:36 +0300 Subject: [PATCH 10/14] fix(gnss_poser): Update readme Signed-off-by: Ata Parlar --- sensing/gnss_poser/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index c1ea090ab1ca2..51ca562a8ee56 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -28,12 +28,12 @@ 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 | -| `coordinate_system` | int | "4" | coordinate system enumeration; 1: UTM, 2: MGRS, 3: Plane, 4: WGS84 Local Coordinate System, 5: UTM Local Coordinate System | +| `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) | From 4657137c4f64048f742ed89bd396bab5f2c780d8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 20 Jul 2022 10:44:58 +0000 Subject: [PATCH 11/14] ci(pre-commit): autofix --- sensing/gnss_poser/README.md | 2 +- .../gnss_poser/include/gnss_poser/convert.hpp | 60 +++++++++---------- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 51ca562a8ee56..6113fd6e68e43 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -28,7 +28,7 @@ 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 | diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 848f2231b735b..bf6689c870c34 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -100,37 +100,37 @@ 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) + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger) { - GNSSStat utm_origin; - GNSSStat utm_local; - utm_origin.coordinate_system = CoordinateSystem::UTM; - utm_local.coordinate_system = CoordinateSystem::UTM; - double global_x; - double global_y; - try { - // origin of the local coordinate system in global - 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 - 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 utm_origin; + GNSSStat utm_local; + utm_origin.coordinate_system = CoordinateSystem::UTM; + utm_local.coordinate_system = CoordinateSystem::UTM; + double global_x; + double global_y; + try { + // origin of the local coordinate system in global + 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 + 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) From 0f1405989ce2de0d78ebfcef46c256e163e7e3c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Wed, 20 Jul 2022 14:19:08 +0300 Subject: [PATCH 12/14] fix id MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- sensing/gnss_poser/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 6113fd6e68e43..365b46bf15577 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -28,8 +28,8 @@ 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 | +| -------------------- | ------ | ---------------- |--------------------------------------------------------------------------------------------------------------------------------------------| +| `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 | From f1b1e83e2c9a1aeb9e51637f87f343433521d4ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Wed, 20 Jul 2022 14:19:34 +0300 Subject: [PATCH 13/14] move variables closer to the scope they were used MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- sensing/gnss_poser/include/gnss_poser/convert.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index bf6689c870c34..ae74083048c64 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -103,19 +103,19 @@ 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_origin; GNSSStat utm_local; - utm_origin.coordinate_system = CoordinateSystem::UTM; utm_local.coordinate_system = CoordinateSystem::UTM; - double global_x; - double global_y; try { - // origin of the local coordinate system in global + // 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); From 064f1e7c9f8d277782081b9537e4b829c0f6b5d9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 20 Jul 2022 11:21:26 +0000 Subject: [PATCH 14/14] ci(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 365b46bf15577..69925cc43d0f3 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -28,7 +28,7 @@ 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 |