Revert "add local cartesian option for origin value"

This reverts commit 40f88e4.
autowarefoundation · Apr 22, 2022 · 8ef6bd4 · 8ef6bd4
1 parent 2a60dab
commit 8ef6bd4
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 31 deletions.
diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp
@@ -26,7 +26,7 @@
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
 
 #include <string>
-#include <GeographicLib/LocalCartesian.hpp>
+
 namespace gnss_poser
 {
 enum class MGRSPrecision {
@@ -80,29 +80,23 @@ GNSSStat NavSatFix2LocalCartesian(
   return local_cartesian;
 }
 GNSSStat NavSatFix2UTM(
-  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, UtmProjectorType utm_projector_type,sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger)
+  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger)
 {
   GNSSStat utm;
   utm.coordinate_system = CoordinateSystem::UTM;
+
   try {
-    if (utm_projector_type == UtmProjectorType::LocalCartesian) {
-      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, utm.x, utm.y,
-        utm.z);
-    } else if (utm_projector_type == UtmProjectorType::UTMUPS) {
-      GeographicLib::UTMUPS::Forward(
-        nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.northup, utm.x, utm.y);
+    GeographicLib::UTMUPS::Forward(
+      nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.northup, utm.x, utm.y);
+
+    utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger);
 
-      utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger);
-    }
     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());
-    }
+    RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what());
+  }
   return utm;
 }
 
@@ -135,10 +129,10 @@ GNSSStat UTM2MGRS(
 }
 
 GNSSStat NavSatFix2MGRS(
-  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, UtmProjectorType & utm_projector_type, sensor_msgs::msg::NavSatFix & nav_sat_fix_origin, const MGRSPrecision & precision,
+  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision,
   const rclcpp::Logger & logger)
 {
-  const auto utm = NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type,nav_sat_fix_origin, logger);
+  const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger);
   const auto mgrs = UTM2MGRS(utm, precision, logger);
   return mgrs;
 }

diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp
@@ -79,7 +79,6 @@ class GNSSPoser : public rclcpp::Node
   rclcpp::Publisher<tier4_debug_msgs::msg::BoolStamped>::SharedPtr fixed_pub_;
 
   CoordinateSystem coordinate_system_;
-  UtmProjectorType utm_projector_type;
   std::string base_frame_;
   std::string gnss_frame_;
   std::string gnss_base_frame_;

diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp
@@ -23,11 +23,6 @@ enum class CoordinateSystem {
   LOCAL_CARTESIAN = 3,
 };
 
-enum class UtmProjectorType{
-  UTMUPS = 0,
-  LocalCartesian = 1,
-};
-
 struct GNSSStat
 {
   GNSSStat()

diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml
@@ -10,7 +10,7 @@
 
   <arg name="base_frame" default="base_link"/>
   <arg name="gnss_base_frame" default="gnss_base_link"/>
-  <arg name="gnss_frame" default="ins_corrected"/>
+  <arg name="gnss_frame" default="gnss"/>
   <arg name="map_frame" default="map"/>
 
   <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
@@ -32,7 +32,6 @@
     <param name="map_frame" value="$(var map_frame)"/>
 
     <param name="coordinate_system" value="$(var coordinate_system)"/>
-    <param name="utm_projector_type" value="$(var utm_projector_type)"/>
     <param name="buff_epoch" value="$(var buff_epoch)"/>
     <param name="use_ublox_receiver" value="$(var use_ublox_receiver)"/>
     <param name="plane_zone" value="$(var plane_zone)"/>

diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp
@@ -34,7 +34,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
 {
   int coordinate_system =
     declare_parameter("coordinate_system", static_cast<int>(CoordinateSystem::MGRS));
-
   coordinate_system_ = static_cast<CoordinateSystem>(coordinate_system);
 
   nav_sat_fix_origin.latitude = declare_parameter("latitude", 0.0);
@@ -164,14 +163,11 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix
 GNSSStat GNSSPoser::convert(
   const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system)
 {
-
-
-
   GNSSStat gnss_stat;
   if (coordinate_system == CoordinateSystem::UTM) {
-    gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type,nav_sat_fix_origin, this->get_logger());
+    gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger());
   } else if (coordinate_system == CoordinateSystem::MGRS) {
-    gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, utm_projector_type, nav_sat_fix_origin, MGRSPrecision::_100MICRO_METER, this->get_logger());
+    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) {