add local cartesian option for origin value

Signed-off-by: melike tanrikulu <melike@leodrive.ai>

sensing/gnss_poser/CMakeLists.txt

@@ -52,4 +52,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

sensing/gnss_poser/config/set_local_origin.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+
+    # Latitude of local origin
+    latitude: 40.816617984672746
+
+    # Longitude of local origin
+    longitude: 29.360491808334285
+
+    # Altitude of local origin
+    altitude: 52.251157145314075

sensing/gnss_poser/include/gnss_poser/convert.hpp

@@ -25,7 +25,7 @@
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
 
 #include <string>
-
+#include <GeographicLib/LocalCartesian.hpp>
 namespace gnss_poser
 {
 enum class MGRSPrecision {
@@ -58,23 +58,29 @@ double EllipsoidHeight2OrthometricHeight(
 }
 
 GNSSStat NavSatFix2UTM(
-  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger)
+  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)
 {
   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);
+    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);
 
+      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;
 }
 
@@ -107,10 +113,10 @@ GNSSStat UTM2MGRS(
 }
 
 GNSSStat NavSatFix2MGRS(
-  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision,
+  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 rclcpp::Logger & logger)
 {
-  const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger);
+  const auto utm = NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type,nav_sat_fix_origin, logger);
   const auto mgrs = UTM2MGRS(utm, precision, logger);
   return mgrs;
 }

sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp

@@ -79,11 +79,12 @@ 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_;
   std::string map_frame_;
-
+  sensor_msgs::msg::NavSatFix nav_sat_fix_origin;
   bool use_ublox_receiver_;
 
   int plane_zone_;

sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp

@@ -22,6 +22,11 @@ enum class CoordinateSystem {
   PLANE = 2,
 };
 
+enum class UtmProjectorType{
+  UTMUPS = 0,
+  LocalCartesian = 1,
+};
+
 struct GNSSStat
 {
   GNSSStat()

sensing/gnss_poser/launch/gnss_poser.launch.xml

@@ -2,17 +2,19 @@
 <launch>
   <arg name="input_topic_fix" default="/fix"/>
   <arg name="input_topic_navpvt" default="/navpvt"/>
+  <arg name="param_file" default="$(find-pkg-share gnss_poser)/config/set_local_origin.param.yaml"/>
 
   <arg name="output_topic_gnss_pose" default="gnss_pose"/>
   <arg name="output_topic_gnss_pose_cov" default="gnss_pose_cov"/>
   <arg name="output_topic_gnss_fixed" default="gnss_fixed"/>
 
   <arg name="base_frame" default="base_link"/>
   <arg name="gnss_base_frame" default="gnss_base_link"/>
-  <arg name="gnss_frame" default="gnss"/>
+  <arg name="gnss_frame" default="ins_corrected"/>
   <arg name="map_frame" default="map"/>
 
-  <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE"/>
+  <arg name="coordinate_system" default="0" description="0:UTM, 1:MGRS, 2:PLANE"/>
+  <arg name="utm_projector_type" default="1" description="0:UPS, 1:LocalCartesian"/>
   <arg name="buff_epoch" default="1"/>
   <arg name="use_ublox_receiver" default="false"/>
   <arg name="plane_zone" default="9"/>
@@ -31,8 +33,10 @@
     <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)"/>
+    <param from="$(var param_file)"/>
   </node>
 </launch>

sensing/gnss_poser/src/gnss_poser_core.cpp

@@ -34,8 +34,15 @@ 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);
 
+  int utm_projector_type_ = declare_parameter("utm_projector_type",static_cast<int>(UtmProjectorType::LocalCartesian));
+  utm_projector_type = static_cast<UtmProjectorType>(utm_projector_type_);
+  nav_sat_fix_origin.latitude  = declare_parameter("latitude", 0.0);
+  nav_sat_fix_origin.longitude = declare_parameter("longitude", 0.0);
+  nav_sat_fix_origin.altitude  = declare_parameter("altitude", 0.0);
+
   int buff_epoch = declare_parameter("buff_epoch", 1);
   position_buffer_.set_capacity(buff_epoch);
 
@@ -159,11 +166,14 @@ 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, this->get_logger());
+    gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type,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());
+    gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, utm_projector_type, nav_sat_fix_origin, 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 {