From 9f4cc8295667a67be8314b2120d66141c0bb4db5 Mon Sep 17 00:00:00 2001
From: Marius-Juston <marius.juston@hotmail.fr>
Date: Sat, 20 Feb 2021 22:01:58 -0500
Subject: [PATCH] added backup in case tf is not found

---
 src/ukf_uwb_localization.py | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/ukf_uwb_localization.py b/src/ukf_uwb_localization.py
index 116427d..0864aef 100644
--- a/src/ukf_uwb_localization.py
+++ b/src/ukf_uwb_localization.py
@@ -49,7 +49,14 @@ def retrieve_tag_offsets(self, tags, base_link='base_link'):
 
         rate = rospy.Rate(10.0)
 
+        default = {
+            1: np.array([0, 0.162, 0.184]),
+            0: np.array([0, -0.162, 0.184])
+        }
+
         for tag in tags:
+            timeout = 5
+
             while not rospy.is_shutdown():
                 try:
                     (trans,rot) = listener.lookupTransform(base_link, tag, rospy.Time(0))
@@ -57,7 +64,11 @@ def retrieve_tag_offsets(self, tags, base_link='base_link'):
                     break
 
                 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
-                    continue
+                    if timeout <= 0:
+                        transforms[tags[tag]] = default[tags[tag]]
+                        break
+                    timeout -= 1
+
 
                 rate.sleep()