tolerate missing tf data temporarily

MOLAorg · Mar 19, 2024 · 86caa90 · 86caa90
1 parent c9cddcb
commit 86caa90
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 7 deletions.
diff --git 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
@@ -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,15 +289,19 @@ 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);
         }
         else if (sensorType == "CObservation2DRangeScan")
         {
             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,31 +310,38 @@ 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);
         }
         else if (sensorType == "CObservationIMU")
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toIMU(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toIMU(sensorLabel, m, fixedSensorPose);
+                    });
                 };
             lookup_[topic].emplace_back(callback);
         }
         else if (sensorType == "CObservationGPS")
         {
             auto callback =
                 [=](const rosbag2_storage::SerializedBagMessage& m) {
-                    return toGPS(sensorLabel, m, fixedSensorPose);
+                    return catchExceptions([=]() {
+                        return toGPS(sensorLabel, m, fixedSensorPose);
+                    });
                 };
             lookup_[topic].emplace_back(callback);
         }
         else if (sensorType == "CObservationOdometry")
         {
             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 {};
+    }
+}