From 44af54170f5bab7fe38c7ca7b4289249edf06262 Mon Sep 17 00:00:00 2001 From: HashimHS Date: Tue, 17 Dec 2024 18:43:38 +0100 Subject: [PATCH] Added: Element type checks Fixed: Type conflict with tf_ros (Changed from import msg to tf2_geometry msgs) Workaround: Spinning now works (hacky solution until a better solution is found) --- .../aau_spatial_reasoner.py | 61 ++++++++++++------- 1 file changed, 40 insertions(+), 21 deletions(-) diff --git a/skiros2_std_reasoners/skiros2_std_reasoners/aau_spatial_reasoner.py b/skiros2_std_reasoners/skiros2_std_reasoners/aau_spatial_reasoner.py index 5e10953..6e50bd1 100644 --- a/skiros2_std_reasoners/skiros2_std_reasoners/aau_spatial_reasoner.py +++ b/skiros2_std_reasoners/skiros2_std_reasoners/aau_spatial_reasoner.py @@ -1,9 +1,12 @@ from skiros2_common.core.discrete_reasoner import DiscreteReasoner import skiros2_common.tools.logger as log +from skiros2_common.core.world_element import Element import tf2_ros as tf from .transformations import euler_from_quaternion, quaternion_from_euler -from geometry_msgs.msg import Pose, TransformStamped, PoseStamped +# from geometry_msgs.msg import Pose, TransformStamped, PoseStamped +from tf2_geometry_msgs import Pose, TransformStamped, PoseStamped import rclpy +from rclpy.node import Node from rclpy.duration import Duration from rclpy.time import Time import numpy @@ -13,7 +16,7 @@ class AauSpatialReasoner(DiscreteReasoner): - def init_ros(self, node): + def init_ros(self, node: Node): """ @brief Reasoner handling transformations and spatial relations @@ -49,7 +52,7 @@ def init_ros(self, node): self._tlb = tf.Buffer() self._tl = tf.TransformListener(self._tlb, self._node) - def parse(self, element, action): + def parse(self, element: Element, action): """ @brief Called by the world model every time an objects is modified @@ -79,7 +82,7 @@ def _reset(self): self._tf_list = {} self._linked_list = {} self._to_rebase_list = {} - root = self._wmi.get_element("skiros:Scene-0") + root: Element = self._wmi.get_element("skiros:Scene-0") self._spatial_rels = self._wmi.get_sub_properties("skiros:spatiallyRelated") if root.hasProperty("skiros:FrameId"): self._base_frame = root.getProperty("skiros:FrameId").value @@ -91,7 +94,7 @@ def _reset(self): for e in self._wmi.get_recursive(root.id, "skiros:spatiallyRelated").values(): self.parse(e, "add") - def _getParentFrame(self, e): + def _getParentFrame(self, e: Element): """ @brief Gets the parent frame in the skiros world scene @@ -103,7 +106,7 @@ def _getParentFrame(self, e): if not c_rel: raise Exception("Element {} has not parent. Debug: {}".format( e.printState(), e.printState(True))) - parent = self._wmi.get_element(c_rel['src']) + parent: Element = self._wmi.get_element(c_rel['src']) if parent.id in self._tf_list or parent.id in self._linked_list or parent.id == "skiros:Scene-0": return parent.getProperty("skiros:FrameId").value else: @@ -122,7 +125,8 @@ def get_transform(self, base_frm, target_frm, duration=Duration(nanoseconds=0.0) fails. """ try: - t = self._tlb.lookup_transform(base_frm, target_frm, Time(), duration) + # t = self._tlb.lookup_transform(base_frm, target_frm, Time(), duration) + t = self.spin_lookup_transform(base_frm, target_frm, Time(), duration) # Temporary fix return ((t.transform.translation.x, t.transform.translation.y, t.transform.translation.z), (t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: @@ -147,8 +151,9 @@ def transform(self, element, target_frm, duration=Duration(nanoseconds=0.0)): self._node.get_logger().warn("[{}] {} failed to transform. Empty source '{}' or target '{}' frame.".format( self.__class__.__name__, element, pose.header.frame_id, target_frm)) return False - self._tlb.lookup_transform(target_frm, pose.header.frame_id, - pose.header.stamp, duration) + # self._tlb.lookup_transform(target_frm, pose.header.frame_id, + # pose.header.stamp, duration) + self.spin_lookup_transform(target_frm, pose.header.frame_id, pose.header.stamp, duration) # Temporary fix element.setData(":PoseStampedMsg", self._tlb.transform(pose, target_frm)) element.setProperty("skiros:TfTimeStamp", None) log.info(self.__class__.__name__, "{} transformed from base {} to base {}".format( @@ -166,7 +171,7 @@ def _update_linked_objects(self): """ for k in list(self._linked_list.keys()): self.k = k - e = self._wmi.get_element(k) + e: Element = self._wmi.get_element(k) base_frm = e.getProperty("skiros:BaseFrameId").value if not e.hasProperty("skiros:LinkedToFrameId", not_none=True) or \ e.hasProperty("skiros:LinkedToFrameId", ""): @@ -188,7 +193,7 @@ def _update_linked_objects(self): self._wmi.update_element(e, self.__class__.__name__) self._e_to_update = list() - def _publishTransform(self, e): + def _publishTransform(self, e: Element): """ @brief Publish the element pose on tf @@ -225,7 +230,7 @@ def _quaternion_normalize(self, q): return q return q / norm - def _trigger_children_update(self, e): + def _trigger_children_update(self, e: Element): """ @brief Children of e get scheduled for a check of the parent frame """ @@ -271,7 +276,7 @@ def _process_to_rebase(self): self._register(element, parent_frame) self._wmi.update_properties(element, self.__class__.__name__, self) - def _register(self, element, parent_frame): + def _register(self, element: Element, parent_frame): """ @brief Adds an element to tf publish list """ @@ -286,7 +291,7 @@ def _register(self, element, parent_frame): element._last_tf_timestamp = self._node.get_clock().now() self._tf_list[element.id] = element - def _unregister(self, element, set_publish_property=True): + def _unregister(self, element: Element, set_publish_property=True): """ @brief Removes an element from tf publish list """ @@ -297,7 +302,7 @@ def _unregister(self, element, set_publish_property=True): del self._tf_list[element.id] self._trigger_children_update(element) - def _format_element(self, element): + def _format_element(self, element: Element): """ @brief Format element properties and register to tf publish list """ @@ -346,7 +351,7 @@ def run(self): self._publish_tf_list() rate.sleep() - def onAddProperties(self, element): + def onAddProperties(self, element: Element): """ @brief Add default reasoner properties to the element """ @@ -379,7 +384,7 @@ def onAddProperties(self, element): if not element.hasProperty("skiros:SizeZ"): element.setProperty("skiros:SizeZ", float) - def onRemoveProperties(self, element): + def onRemoveProperties(self, element: Element): """ @brief Remove default reasoner properties to the element """ @@ -387,7 +392,7 @@ def onRemoveProperties(self, element): if element.hasProperty(k): element.removeProperty(k) - def hasData(self, element, get_code): + def hasData(self, element: Element, get_code): """ @brief Return true if the data is available in the element """ @@ -410,7 +415,7 @@ def hasData(self, element, get_code): log.error("[AauSpatialReasoner] Code {} not recognized".format(get_code)) return False - def getData(self, element, get_code): + def getData(self, element: Element, get_code): """ @brief Return data from the element in the format indicated in get_code """ @@ -479,7 +484,7 @@ def getData(self, element, get_code): log.error("[AauSpatialReasoner] Code {} not recognized".format(get_code)) return None - def setData(self, element, data, set_code): + def setData(self, element: Element, data, set_code): """ @brief Convert user data to reasoner data and store it into given element """ @@ -680,7 +685,8 @@ def computeRelations(self, sub, obj, with_metrics=False): if obj.getProperty("skiros:FrameId").value == "": obj.setProperty("skiros:FrameId", "obj_temp_frame") self._tlb.settransform(self.getData(obj, ":TransformMsg"), "AauSpatialReasoner") - self._tlb.lookup_transform(obj_base_frame, sub_frame, Time(), Duration(nanoseconds=10**9)) + # self._tlb.lookup_transform(obj_base_frame, sub_frame, Time(), Duration(nanoseconds=10**9)) + self.spin_lookup_transform(obj_base_frame, sub_frame, Time(), Duration(seconds=1)) # Temporary fix obj.setData(":PoseStampedMsg", self._tlb.transform(obj.getData(":PoseStampedMsg"), sub_frame)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): log.error("[computeRelations]", "Couldn't transform object in frame {} to frame {}.".format( @@ -716,3 +722,16 @@ def computeRelations(self, sub, obj, with_metrics=False): to_ret.append(self._get_orientation_relation(oo)[0]) # print to_ret return to_ret + + def spin_lookup_transform(self, to_frame, from_frame, stamp=Time(), duration=Duration(seconds=0.1)): + """ + @brief Temporarily fix for spinning + """ + if duration == Duration(seconds=0.0): + duration = Duration(seconds=0.1) + time_start = self._node.get_clock().now() + while not self._tlb.can_transform(to_frame, from_frame, stamp): + rclpy.spin_once(self._node, timeout_sec=0.03) + if time_start + duration < self._node.get_clock().now(): + raise Exception("Timeout, could not find transformation between {} and {}".format(from_frame, to_frame)) + return self._tlb.lookup_transform(to_frame, from_frame, stamp, duration) \ No newline at end of file