From 9ecdc49164e32f156d9c347a3cd741479b844e5f Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Thu, 5 Dec 2024 20:11:54 +0100
Subject: [PATCH] ROS1 rosbag2rawlog: Add support for GPS

---
 apps/rosbag2rawlog/rosbag2rawlog_main.cpp | 34 +++++++++++++++++++++++
 doc/source/doxygen-docs/changelog.md      |  2 ++
 2 files changed, 36 insertions(+)

diff --git a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp
index f7c64bcd16..dde641b4ee 100644
--- a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp
+++ b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp
@@ -30,6 +30,7 @@
 #include <mrpt/obs/CObservationPointCloud.h>
 #include <mrpt/obs/CObservationRotatingScan.h>
 #include <mrpt/poses/CPose3DQuat.h>
+#include <mrpt/ros1bridge/gps.h>
 #include <mrpt/ros1bridge/imu.h>
 #include <mrpt/ros1bridge/laser_scan.h>
 #include <mrpt/ros1bridge/point_cloud2.h>
@@ -398,6 +399,33 @@ Obs toIMU(
   return {mrptObs};
 }
 
+Obs toGPS(
+    std::string_view msg,
+    const rosbag::MessageInstance& rosmsg,
+    const std::optional<mrpt::poses::CPose3D>& fixedSensorPose)
+{
+  auto gps = rosmsg.instantiate<sensor_msgs::NavSatFix>();
+
+  auto mrptObs = mrpt::obs::CObservationGPS::Create();
+
+  mrptObs->sensorLabel = msg;
+  mrptObs->timestamp = mrpt::ros1bridge::fromROS(gps->header.stamp);
+
+  // Convert data:
+  mrpt::ros1bridge::fromROS(*gps, *mrptObs);
+
+  bool sensorPoseOK = findOutSensorPose(
+      mrptObs->sensorPose, gps->header.frame_id, arg_base_link_frame.getValue(), fixedSensorPose);
+  if (!sensorPoseOK)
+  {
+    std::cerr << "Warning: dropping one observation of type '" << msg
+              << "' due to missing /tf data.\n";
+    return {};
+  }
+
+  return {mrptObs};
+}
+
 Obs toOdometry(std::string_view msg, const rosbag::MessageInstance& rosmsg)
 {
   auto odo = rosmsg.instantiate<nav_msgs::Odometry>();
@@ -676,6 +704,12 @@ class Transcriber
         { return toIMU(sensorName, m, fixedSensorPose); };
         m_lookup[sensor.at("topic").as<std::string>()].emplace_back(callback);
       }
+      else if (sensorType == "CObservationGPS")
+      {
+        auto callback = [=](const rosbag::MessageInstance& m)
+        { return toGPS(sensorName, m, fixedSensorPose); };
+        m_lookup[sensor.at("topic").as<std::string>()].emplace_back(callback);
+      }
       else if (sensorType == "CObservationOdometry")
       {
         auto callback = [=](const rosbag::MessageInstance& m) { return toOdometry(sensorName, m); };
diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
index 8771f001b3..b659979e4d 100644
--- a/doc/source/doxygen-docs/changelog.md
+++ b/doc/source/doxygen-docs/changelog.md
@@ -1,6 +1,8 @@
 \page changelog Change Log
 
 # Version 2.14.7: UNRELEASED
+- Changes in apps:
+  - rosbag2rawlog (ROS1): Implement conversion of NavSatFix -> mrpt::obs::CObservationGPS
 - Build system:
   - `mrpt-*-config.cmake` files now enforce the search of cmake dependencies in CONFIG mode, to avoid being foolish by deprecated `FindXXX()` lying around.