diff --git a/skiros2_std_reasoners/src/skiros2_std_reasoners/aau_spatial_reasoner.py b/skiros2_std_reasoners/src/skiros2_std_reasoners/aau_spatial_reasoner.py index cd608d6..94e99d3 100644 --- a/skiros2_std_reasoners/src/skiros2_std_reasoners/aau_spatial_reasoner.py +++ b/skiros2_std_reasoners/src/skiros2_std_reasoners/aau_spatial_reasoner.py @@ -47,6 +47,7 @@ def __init__(self): """ self._tlb = tf.Buffer() self._tl = tf.TransformListener(self._tlb) + self._create_time = rospy.Time.now() def parse(self, element, action): """ @@ -120,6 +121,9 @@ def get_transform(self, base_frm, target_frm, duration=rospy.Duration(0.0)): orientation quaternion. (None, None) if transformation fails. """ + # Grace period to fill the buffer + if (rospy.Time.now() - self._create_time) < rospy.Duration.from_sec(0.3): + rospy.sleep(rospy.Duration.from_sec(0.3)-(rospy.Time.now() - self._create_time)) try: t = self._tlb.lookup_transform(base_frm, target_frm, rospy.Time(0), duration) return ((t.transform.translation.x, t.transform.translation.y, t.transform.translation.z), @@ -140,6 +144,9 @@ def transform(self, element, target_frm, duration=rospy.Duration(0.0)): @return True if pose was changed, False otherwise """ + # Grace period to fill the buffer + if (rospy.Time.now() - self._create_time) < rospy.Duration.from_sec(0.3): + rospy.sleep(rospy.Duration.from_sec(0.3)-(rospy.Time.now() - self._create_time)) try: pose = element.getData(":PoseStampedMsg") self._tlb.lookup_transform(target_frm, pose.header.frame_id,