From 86caa9046551cf4111c3edc3b65edbf624e391db Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Tue, 19 Mar 2024 07:30:38 +0100
Subject: [PATCH] tolerate missing tf data temporarily

---
 .../mola_input_rosbag2/Rosbag2Dataset.h       |  2 +
 mola_input_rosbag2/src/Rosbag2Dataset.cpp     | 44 ++++++++++++++++---
 2 files changed, 39 insertions(+), 7 deletions(-)

diff --git a/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h b/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h
index 54285b48..6d108c80 100644
--- a/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h
+++ b/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h
@@ -198,6 +198,8 @@ class Rosbag2Dataset : public RawDataSourceBase,
         const rosbag2_storage::SerializedBagMessage& rosmsg,
         const std::optional<mrpt::poses::CPose3D>&   fixedSensorPose);
 
+    Obs catchExceptions(const std::function<Obs()>& f);
+
     bool findOutSensorPose(
         mrpt::poses::CPose3D& des, const std::string& target_frame,
         const std::string&                         source_frame,
diff --git a/mola_input_rosbag2/src/Rosbag2Dataset.cpp b/mola_input_rosbag2/src/Rosbag2Dataset.cpp
index b8872b37..fcdd9203 100644
--- a/mola_input_rosbag2/src/Rosbag2Dataset.cpp
+++ b/mola_input_rosbag2/src/Rosbag2Dataset.cpp
@@ -267,7 +267,9 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toPointCloud2(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toPointCloud2(sensorLabel, m, fixedSensorPose);
+                    });
                 };
             lookup_[topic].emplace_back(callback);
         }
@@ -287,7 +289,9 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toImage(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toImage(sensorLabel, m, fixedSensorPose);
+                    });
                 };
             lookup_[topic].emplace_back(callback);
         }
@@ -295,7 +299,9 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toLidar2D(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toLidar2D(sensorLabel, m, fixedSensorPose);
+                    });
                 };
 
             lookup_[topic].emplace_back(callback);
@@ -304,7 +310,9 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toRotatingScan(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toRotatingScan(sensorLabel, m, fixedSensorPose);
+                    });
                 };
             lookup_[topic].emplace_back(callback);
         }
@@ -312,7 +320,9 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toIMU(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toIMU(sensorLabel, m, fixedSensorPose);
+                    });
                 };
             lookup_[topic].emplace_back(callback);
         }
@@ -320,7 +330,9 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toGPS(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toGPS(sensorLabel, m, fixedSensorPose);
+                    });
                 };
             lookup_[topic].emplace_back(callback);
         }
@@ -328,7 +340,8 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toOdometry(sensorLabel, m);
+                    return catchExceptions(
+                        [=]() { return toOdometry(sensorLabel, m); });
                 };
             lookup_[topic].emplace_back(callback);
         }
@@ -919,3 +932,20 @@ Rosbag2Dataset::SF::Ptr Rosbag2Dataset::to_mrpt(
     }
     return rets;
 }  // end to_mrpt()
+
+Rosbag2Dataset::Obs Rosbag2Dataset::catchExceptions(
+    const std::function<Obs()>& f)
+{
+    try
+    {
+        return f();
+    }
+    catch (const std::exception& e)
+    {
+        MRPT_LOG_ERROR_STREAM(
+            "Exception while processing topic message (ignore if the error "
+            "stops later one, e.g. missing /tf):\n"
+            << e.what());
+        return {};
+    }
+}