added backup in case tf is not found

sehb · Feb 21, 2021 · 9f4cc82 · 9f4cc82
1 parent 3f4880b
commit 9f4cc82
Showing 1 changed file with 12 additions and 1 deletion.
diff --git a/src/ukf_uwb_localization.py b/src/ukf_uwb_localization.py
@@ -49,15 +49,26 @@ 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))
                     transforms[tags[tag]] = np.array([trans[0], trans[1]])
                     break
 
                 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
-                    continue
+                    if timeout <= 0:
+                        transforms[tags[tag]] = default[tags[tag]]
+                        break
+                    timeout -= 1
+
 
                 rate.sleep()