From 7d5b216ab98156e7be1f7c75732c7f32149bf63a Mon Sep 17 00:00:00 2001
From: Dongmyeong Lee <domlee@utexas.edu>
Date: Fri, 3 Nov 2023 20:13:54 -0500
Subject: [PATCH] Add fallback when TF tree for laserscan is not published

---
 src/navigation/navigation_main.cc | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index 8d61272..bf77716 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -212,7 +212,12 @@ void RetrieveTransform(const std_msgs::Header& msg,
                                   transform.getRotation().getY(),
                                   transform.getRotation().getZ());
   } catch (tf::TransformException& ex) {
-    ROS_ERROR("%s", ex.what());
+    ROS_WARN("Failed to retrieve transform from '%s' to '%s': %s",
+             msg.frame_id.c_str(), CONFIG_laser_frame.c_str(), ex.what());
+    ROS_INFO("Assuming '%s' frame is equal to '%s' frame;",
+             msg.frame_id.c_str(), CONFIG_laser_frame.c_str());
+    // Set frame_tf to identity as a fallback.
+    frame_tf = Eigen::Affine3f::Identity();
   }
 }