fix(gnss_poser): add local cartesian option for origin value (tier4#725)

Signed-off-by: melike tanrikulu <melike@leodrive.ai>
boyali · Sep 28, 2022 · d2ad0cf · d2ad0cf
1 parent 5fd7e84
commit d2ad0cf
Show file tree

Hide file tree

Showing 7 changed files with 45 additions and 1 deletion.
diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt
@@ -37,4 +37,5 @@ rclcpp_components_register_node(gnss_poser_node
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )
diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/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
diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp
@@ -17,6 +17,7 @@
 #include "gnss_poser/gnss_stat.hpp"
 
 #include <GeographicLib/Geoid.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
 #include <GeographicLib/MGRS.hpp>
 #include <GeographicLib/UTMUPS.hpp>
 #include <geo_pos_conv/geo_pos_conv.hpp>
@@ -56,7 +57,28 @@ double EllipsoidHeight2OrthometricHeight(
   }
   return OrthometricHeight;
 }
+GNSSStat NavSatFix2LocalCartesian(
+  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;
+
+  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)
 {

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

diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp
@@ -20,6 +20,7 @@ enum class CoordinateSystem {
   UTM = 0,
   MGRS = 1,
   PLANE = 2,
+  LOCAL_CARTESIAN = 3,
 };
 
 struct GNSSStat

diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml
@@ -2,6 +2,7 @@
 <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"/>
@@ -12,7 +13,7 @@
   <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"/>
+  <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"/>
@@ -34,5 +35,6 @@
     <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>
diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp
@@ -36,6 +36,10 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
     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);
+  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);
 
@@ -166,6 +170,8 @@ GNSSStat GNSSPoser::convert(
     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) {
+    gnss_stat = NavSatFix2LocalCartesian(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(),