🎈 perf: Improve TF lookup error handling

Avoid throwing an error and shutdown immediately.
SMBU-PolarBear-Robotics-Team · Dec 24, 2024 · 2fbf1d7 · 2fbf1d7
1 parent 82ae1e8
commit 2fbf1d7
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/loam_interface/src/loam_interface.cpp b/loam_interface/src/loam_interface.cpp
@@ -59,8 +59,8 @@ void LoamInterfaceNode::odometryCallback(const nav_msgs::msg::Odometry::ConstSha
       tf_odom_to_lidar_odom_ = tf_base_frame_to_lidar;
       base_frame_to_lidar_initialized_ = true;
     } catch (tf2::TransformException & ex) {
-      RCLCPP_WARN(this->get_logger(), "TF lookup failed: %s", ex.what());
-      throw;
+      RCLCPP_WARN(this->get_logger(), "TF lookup failed: %s Retrying...", ex.what());
+      return;
     }
   }
 

diff --git a/sensor_scan_generation/src/sensor_scan_generation.cpp b/sensor_scan_generation/src/sensor_scan_generation.cpp
@@ -85,8 +85,8 @@ tf2::Transform SensorScanGenerationNode::getTransform(
     tf2::fromMsg(transform_stamped.transform, transform);
     return transform;
   } catch (tf2::TransformException & ex) {
-    RCLCPP_WARN(this->get_logger(), "TF lookup failed: %s", ex.what());
-    throw;
+    RCLCPP_WARN(this->get_logger(), "TF lookup failed: %s. Returning identity.", ex.what());
+    return tf2::Transform::getIdentity();
   }
 }