feat(gnss_poser): remove local cartesian wgs84 coordinate (autowarefo…

…undation#4726) * feat(gnss_poser): remove local cartesian wgs84 coordinate Signed-off-by: kminoda <[email protected]> * update readme Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
brkay54 · Aug 28, 2023 · bb702a6 · bb702a6
1 parent 6dcd8b0
commit bb702a6
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 35 deletions.
diff --git 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
@@ -17,7 +17,6 @@
 #include "gnss_poser/gnss_stat.hpp"
 
 #include <GeographicLib/Geoid.hpp>
-#include <GeographicLib/LocalCartesian.hpp>
 #include <GeographicLib/MGRS.hpp>
 #include <GeographicLib/UTMUPS.hpp>
 #include <rclcpp/logging.hpp>
@@ -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
@@ -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
@@ -13,7 +13,7 @@
   <arg name="gnss_frame" default="gnss"/>
   <arg name="map_frame" default="map"/>
 
-  <arg name="coordinate_system" default="4" description="1:MGRS, 3:LocalCartesianWGS84, 4:LocalCartesianUTM"/>
+  <arg name="coordinate_system" default="4" description="1:MGRS, 4:LocalCartesianUTM"/>
   <arg name="buff_epoch" default="1"/>
   <arg name="use_gnss_ins_orientation" default="true"/>
   <arg name="height_system" default="1" description="0:Orthometric Height 1:Ellipsoid Height"/>

diff --git 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(),