fix(gnss_poser): updated for space errors

Signed-off-by: melike tanrikulu <melike@leodrive.ai>
autowarefoundation · Apr 20, 2022 · 422f8ef · 422f8ef
1 parent 4ff419d
commit 422f8ef
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 2 deletions.
diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp
@@ -26,6 +26,7 @@
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
 
 #include <string>
+
 namespace gnss_poser
 {
 enum class MGRSPrecision {
@@ -62,6 +63,7 @@ GNSSStat NavSatFix2LocalCartesian(
 {
   GNSSStat local_cartesian;
   local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN;
+
   try {
     GeographicLib::LocalCartesian localCartesian_origin(
       nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, nav_sat_fix_origin.altitude);
@@ -82,11 +84,13 @@ GNSSStat NavSatFix2UTM(
 {
   GNSSStat utm;
   utm.coordinate_system = CoordinateSystem::UTM;
+
   try {
     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.latitude = nav_sat_fix_msg.latitude;
     utm.longitude = nav_sat_fix_msg.longitude;
     utm.altitude = nav_sat_fix_msg.altitude;

diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp
@@ -83,6 +83,7 @@ class GNSSPoser : public rclcpp::Node
   std::string gnss_frame_;
   std::string gnss_base_frame_;
   std::string map_frame_;
+
   sensor_msgs::msg::NavSatFix nav_sat_fix_origin;
   bool use_ublox_receiver_;
 

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="3" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
+  <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
   <arg name="buff_epoch" default="1"/>
   <arg name="use_ublox_receiver" default="false"/>
   <arg name="plane_zone" default="9"/>

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);