From e129a074abd8b391788d3470ef4873961dccde23 Mon Sep 17 00:00:00 2001 From: Walid Ismail Date: Fri, 9 Aug 2024 16:20:42 +0200 Subject: [PATCH 1/2] Move to scipy pyquaternion is not found in the public rosdep rules, see: https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/python.yaml --- tf2_web_republisher_py/package.xml | 2 +- tf2_web_republisher_py/setup.py | 2 +- .../tf2_web_republisher_py/tf_pair.py | 15 +++++++++------ 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/tf2_web_republisher_py/package.xml b/tf2_web_republisher_py/package.xml index 04715bc..fe99ee4 100644 --- a/tf2_web_republisher_py/package.xml +++ b/tf2_web_republisher_py/package.xml @@ -9,7 +9,7 @@ tf2_ros python3-numpy - python3-pyquaternion + python3-scipy ament_copyright ament_flake8 diff --git a/tf2_web_republisher_py/setup.py b/tf2_web_republisher_py/setup.py index 32b85a6..40efb27 100644 --- a/tf2_web_republisher_py/setup.py +++ b/tf2_web_republisher_py/setup.py @@ -11,7 +11,7 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ], - install_requires=['setuptools','numpy','pyquaternion'], + install_requires=['setuptools','numpy','scipy'], zip_safe=True, maintainer='schoen', maintainer_email='schoen.andrewj@gmail.com', diff --git a/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py b/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py index 808c372..e2411a3 100644 --- a/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py +++ b/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py @@ -1,7 +1,7 @@ from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped, Transform import numpy as np -from pyquaternion import Quaternion +from scipy.spatial.transform import Rotation import math import copy @@ -13,8 +13,8 @@ def __init__(self,source_frame,target_frame,angular_thres=0.0,trans_thres=0.0): self._angular_thres = angular_thres self._trans_thres = trans_thres - self._tf_transmitted = {'translation':np.array([0.0,0.0,0.0]),'rotation':Quaternion(1.0,0.0,0.0,0.0)} - self._tf_received = {'translation':np.array([0.0,0.0,0.0]),'rotation':Quaternion(1.0,0.0,0.0,0.0)} + self._tf_transmitted = {'translation':np.array([0.0,0.0,0.0]),'rotation':Rotation.identity()} + self._tf_received = {'translation':np.array([0.0,0.0,0.0]),'rotation':Rotation.identity()} self._last_tf_msg = Transform() @@ -70,7 +70,7 @@ def transmission_triggered(self): def update_transform(self,update:TransformStamped): self._tf_received['translation'] = np.array([update.transform.translation.x,update.transform.translation.y,update.transform.translation.z]) - self._tf_received['rotation'] = Quaternion(update.transform.rotation.w,update.transform.rotation.x,update.transform.rotation.y,update.transform.rotation.z) + self._tf_received['rotation'] = Rotation.from_quat([update.transform.rotation.x, update.transform.rotation.y, update.transform.rotation.z, update.transform.rotation.w]) self._last_tf_msg = update.transform self._updated = True @@ -100,5 +100,8 @@ def distance(tf1,tf2): return np.linalg.norm(tf1['translation']-tf2['translation']) @staticmethod - def angle(tf1,tf2): - return Quaternion.absolute_distance(tf1['rotation'], tf2['rotation']) + def angle(tf1, tf2): + rot1: Rotation = tf1["rotation"] + rot2: Rotation = tf2["rotation"] + relative_rotation = rot1.inv() * rot2 + return np.linalg.norm(relative_rotation.as_rotvec()) From 1920f2e33c959e0010b30523eb3dc4a3e4a8ea09 Mon Sep 17 00:00:00 2001 From: Walid Ismail Date: Fri, 9 Aug 2024 16:29:59 +0200 Subject: [PATCH 2/2] Format & isort with Ruff Also fix minor warnings --- tf2_web_republisher_py/setup.py | 25 +- .../tf2_web_republisher_py.py | 224 +++++++++++------- .../tf2_web_republisher_py/tf_pair.py | 64 +++-- 3 files changed, 198 insertions(+), 115 deletions(-) diff --git a/tf2_web_republisher_py/setup.py b/tf2_web_republisher_py/setup.py index 40efb27..ea12f25 100644 --- a/tf2_web_republisher_py/setup.py +++ b/tf2_web_republisher_py/setup.py @@ -1,26 +1,25 @@ from setuptools import setup -package_name = 'tf2_web_republisher_py' +package_name = "tf2_web_republisher_py" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools','numpy','scipy'], + install_requires=["setuptools", "numpy", "scipy"], zip_safe=True, - maintainer='schoen', - maintainer_email='schoen.andrewj@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="schoen", + maintainer_email="schoen.andrewj@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'tf2_web_republisher = tf2_web_republisher_py.tf2_web_republisher_py:main' + "console_scripts": [ + "tf2_web_republisher = tf2_web_republisher_py.tf2_web_republisher_py:main" ], }, ) diff --git a/tf2_web_republisher_py/tf2_web_republisher_py/tf2_web_republisher_py.py b/tf2_web_republisher_py/tf2_web_republisher_py/tf2_web_republisher_py.py index 8acaabb..ef0904f 100644 --- a/tf2_web_republisher_py/tf2_web_republisher_py/tf2_web_republisher_py.py +++ b/tf2_web_republisher_py/tf2_web_republisher_py/tf2_web_republisher_py.py @@ -1,34 +1,46 @@ +from time import sleep +from typing import List +from uuid import uuid4 + import rclpy -from rclpy.node import Node +from geometry_msgs.msg import TransformStamped from rclpy.action import ActionServer -from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from tf2_ros import Buffer, TransformListener + +from tf2_web_republisher.action import TFSubscription from tf2_web_republisher.msg import TFArray from tf2_web_republisher.srv import RepublishTFs -from tf2_web_republisher.action import TFSubscription -from geometry_msgs.msg import TransformStamped, Transform from tf2_web_republisher_py.tf_pair import TFPair -from tf2_ros import Buffer, TransformListener, TransformException -from rclpy.executors import MultiThreadedExecutor -from typing import List -from uuid import uuid4 -from time import sleep class ClientInfo(object): - def __init__(self,client_id:int,tf_subscriptions:List[TFPair]=[],timer=None): + def __init__(self, client_id: int, tf_subscriptions: List[TFPair] = [], timer=None): self.client_id = client_id self.tf_subscriptions = tf_subscriptions self.timer = timer + class ClientGoalInfo(ClientInfo): - def __init__(self,handle,client_id:int,tf_subscriptions:List[TFPair]=[],timer=None): - super(ClientGoalInfo,self).__init__(client_id,tf_subscriptions,timer) + def __init__( + self, handle, client_id: int, tf_subscriptions: List[TFPair] = [], timer=None + ): + super(ClientGoalInfo, self).__init__(client_id, tf_subscriptions, timer) self.handle = handle + class ClientRequestInfo(ClientInfo): - def __init__(self,client_id:int, node, publisher, tf_subscriptions:List[TFPair]=[],timer=None,timeout:float=10.0): - super(ClientRequestInfo,self).__init__(client_id,tf_subscriptions,timer) - self.publisher = publisher + def __init__( + self, + client_id: int, + node, + publisher, + tf_subscriptions: List[TFPair] = [], + timer=None, + timeout: float = 10.0, + ): + super(ClientRequestInfo, self).__init__(client_id, tf_subscriptions, timer) + self.publisher = publisher self.timeout = timeout self.last_time_since_subscribers = None self.node = node @@ -36,135 +48,169 @@ def __init__(self,client_id:int, node, publisher, tf_subscriptions:List[TFPair]= def __del__(self): self.node.destroy_publisher(self.publisher) + class TFRepublisher(Node): - ''' + """ Python port of the TFRepublisher C++ Node for ROS2 - ''' + """ + def __init__(self): - super(TFRepublisher,self).__init__('tf2_web_republisher') - self.tf_transform_server = ActionServer(node=self, - action_type=TFSubscription, - action_name='tf2_web_republisher', - execute_callback=self.goal_cb, - cancel_callback=self.cancel_cb) - self.republish_service = self.create_service(RepublishTFs, 'republish_tfs', self.request_cb) + super(TFRepublisher, self).__init__("tf2_web_republisher") + self.tf_transform_server = ActionServer( + node=self, + action_type=TFSubscription, + action_name="tf2_web_republisher", + execute_callback=self.goal_cb, + cancel_callback=self.cancel_cb, + ) + self.republish_service = self.create_service( + RepublishTFs, "republish_tfs", self.request_cb + ) self.active_clients = {} - def goal_cb(self,goal_handle): + def goal_cb(self, goal_handle): goal = goal_handle.request client_id = str(uuid4()) - self.get_logger().info("Started action with id" + str(client_id) ) - goal_info = ClientGoalInfo(goal_handle,client_id) - self.set_subscriptions(goal_info, - goal.source_frames, - goal.target_frame, - goal.angular_thres, - goal.trans_thres) - goal_rate = goal.rate + self.get_logger().info("Started action with id" + str(client_id)) + goal_info = ClientGoalInfo(goal_handle, client_id) + self.set_subscriptions( + goal_info, + goal.source_frames, + goal.target_frame, + goal.angular_thres, + goal.trans_thres, + ) + goal_rate = goal.rate if goal_rate == 0: - # If the goal rate is not specified, set it to 10Hz + # If the goal rate is not specified, set it to 10Hz goal_rate = 10.0 - goal_info.timer = self.create_timer(1.0/goal_rate,lambda: self.process_goal(client_id)) + goal_info.timer = self.create_timer( + 1.0 / goal_rate, lambda: self.process_goal(client_id) + ) if not self.active_clients: self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer,node=self) + self.tf_listener = TransformListener(self.tf_buffer, node=self) self.active_clients[client_id] = goal_info # Wait for the action to be cancelled by the client while client_id in self.active_clients: sleep(0.1) - if not self.active_clients : + if not self.active_clients: # Stop listening to tf to save cpu self.tf_listener = None self.tf_buffer = None - self.get_logger().info("Completed action with id" + str(client_id) ) + self.get_logger().info("Completed action with id" + str(client_id)) result = TFSubscription.Result() goal_handle.canceled() return result - def cancel_cb(self,goal_handle): + def cancel_cb(self, goal_handle): client_id_to_be_canceled = None for client_id, goal_info in self.active_clients.items(): if goal_handle == goal_info.handle: client_id_to_be_canceled = client_id break - if client_id_to_be_canceled: + if client_id_to_be_canceled: self.teardown_client(client_id_to_be_canceled) - self.get_logger().info("Cancelling action with id" + str(client_id_to_be_canceled) ) + self.get_logger().info( + "Cancelling action with id" + str(client_id_to_be_canceled) + ) return rclpy.action.CancelResponse.ACCEPT else: - self.get_logger().info("Cancel callback: not found action with id" + str(goal_handle) ) + self.get_logger().info( + "Cancel callback: not found action with id" + str(goal_handle) + ) return rclpy.action.CancelResponse.REJECT - def request_cb(self,request,response): - self.get_logger().info("RepublishTF service request received"); + def request_cb(self, request, response): + self.get_logger().info("RepublishTF service request received") client_id = str(uuid4()).replace("-", "_") - topic_name = 'tf_repub_'+client_id + topic_name = "tf_repub_" + client_id - pub = self.create_publisher(TFArray,topic_name,1 ) + pub = self.create_publisher(TFArray, topic_name, 1) # generate request_info struct request_info = ClientRequestInfo(client_id, self, pub) - self.set_subscriptions(request_info, - request.source_frames, - request.target_frame, - request.angular_thres, - request.trans_thres) + self.set_subscriptions( + request_info, + request.source_frames, + request.target_frame, + request.angular_thres, + request.trans_thres, + ) request_info.timeout = request.timeout request_rate = request.rate if request_rate == 0: - # If the request rate is not specified, set it to 10Hz - request_rate = 10.0 - request_info.timer = self.create_timer(1.0/request_rate,lambda: self.process_request(client_id)) + # If the request rate is not specified, set it to 10Hz + request_rate = 10.0 + request_info.timer = self.create_timer( + 1.0 / request_rate, lambda: self.process_request(client_id) + ) if not self.active_clients: self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer,node=self) + self.tf_listener = TransformListener(self.tf_buffer, node=self) self.active_clients[client_id] = request_info response.topic_name = topic_name - self.get_logger().info('Publishing requested TFs on topic {0}'.format(topic_name)) + self.get_logger().info( + "Publishing requested TFs on topic {0}".format(topic_name) + ) return response @staticmethod - def clean_tf_frame(frame_id:str) -> str: - #if frame_id[0] != '/': + def clean_tf_frame(frame_id: str) -> str: + # if frame_id[0] != '/': # frame_id = '/'+frame_id return frame_id - def set_subscriptions(self,client_info:ClientInfo,source_frames:List[str],target_frame:str,angular_thres:float,trans_thres:float): + def set_subscriptions( + self, + client_info: ClientInfo, + source_frames: List[str], + target_frame: str, + angular_thres: float, + trans_thres: float, + ): client_info.tf_subscriptions = [] for source_frame in source_frames: - tf_pair = TFPair(self.clean_tf_frame(source_frame),self.clean_tf_frame(target_frame),angular_thres,trans_thres) + tf_pair = TFPair( + self.clean_tf_frame(source_frame), + self.clean_tf_frame(target_frame), + angular_thres, + trans_thres, + ) client_info.tf_subscriptions.append(tf_pair) - def teardown_client(self,client_id): + def teardown_client(self, client_id): self.get_logger().info("Tearing down publisher") client = self.active_clients[client_id] client.timer.destroy() del self.active_clients[client_id] - if not self.active_clients : + if not self.active_clients: # Stop listening to tf to save cpu self.tf_listener = None self.tf_buffer = None - def process_request(self, client_id:str): + def process_request(self, client_id: str): request = None try: request = self.active_clients[client_id] - except: + except KeyError: # The id has already been removed - self.get_logger().info('Trying to access removed request:'.format(client_id)) - return + self.get_logger().info( + "Trying to access removed request: {}".format(client_id) + ) + return # Check whether the publisher has any subscribers. If it does, update the stored time. if request.publisher.get_subscription_count() > 0: request.last_time_since_subscribers = self.get_clock().now() elif request.last_time_since_subscribers is None: - return # wait for the first subscriber + return # wait for the first subscriber else: - #elif (self.get_clock().now() - request.last_time_since_subscribers).to_sec() > request.timeout.sec + request.timeout.nanosec * 1000000000: + # elif (self.get_clock().now() - request.last_time_since_subscribers).to_sec() > request.timeout.sec + request.timeout.nanosec * 1000000000: self.teardown_client(client_id) return @@ -172,14 +218,15 @@ def process_request(self, client_id:str): tf_array = self.update_subscriptions(request.tf_subscriptions) if len(tf_array) > 0: # publish TFs - #self.get_logger().debug('Request {0} TFs published:'.format(request.client_id)) + # self.get_logger().debug('Request {0} TFs published:'.format(request.client_id)) tf_array_msg = TFArray() tf_array_msg.transforms = tf_array request.publisher.publish(tf_array_msg) -# else: -# self.get_logger().debug('Request {0} No TF frame update needed:'.format(request.client_id)) - def process_goal(self, client_id:str): + # else: + # self.get_logger().debug('Request {0} No TF frame update needed:'.format(request.client_id)) + + def process_goal(self, client_id: str): request = self.active_clients[client_id] feedback = TFSubscription.Feedback() @@ -187,27 +234,38 @@ def process_goal(self, client_id:str): tf_array = self.update_subscriptions(request.tf_subscriptions) if len(tf_array) > 0: # publish TFs - self.get_logger().info('Client {0} TFs feedback published:'.format(request.client_id)) + self.get_logger().info( + "Client {0} TFs feedback published:".format(request.client_id) + ) feedback.transforms = tf_array request.handle.publish_feedback(feedback) else: - self.get_logger().info('Client {0}: No TF frame update needed:'.format(request.client_id)) + self.get_logger().info( + "Client {0}: No TF frame update needed:".format(request.client_id) + ) - def update_subscriptions(self,tf_subscriptions:List[TFPair]) -> List[TransformStamped]: + def update_subscriptions( + self, tf_subscriptions: List[TFPair] + ) -> List[TransformStamped]: transforms = [] - current_time_msg = self.get_clock().now().to_msg() + current_time_msg = self.get_clock().now().to_msg() # iterate over tf_subscription vector for pair in tf_subscriptions: try: # lookup transformation for tf_pair - transform = self.tf_buffer.lookup_transform(pair.target_frame,pair.source_frame, - rclpy.time.Time()) + transform = self.tf_buffer.lookup_transform( + pair.target_frame, pair.source_frame, rclpy.time.Time() + ) # If the transform broke earlier, but worked now (we didn't get # booted into the catch block), tell the user all is well again if not pair.is_ok: - self.get_logger().info('Transform from {0} to {1} is working again'.format(pair.source_frame,pair.target_frame)) + self.get_logger().info( + "Transform from {0} to {1} is working again".format( + pair.source_frame, pair.target_frame + ) + ) pair.is_ok = True # update tf_pair with transformtion pair.update_transform(transform) @@ -230,16 +288,18 @@ def update_subscriptions(self,tf_subscriptions:List[TFPair]) -> List[TransformSt return transforms + def main(args=[]): rclpy.init(args=args) tf_republisher_node = TFRepublisher() # Use a MultiThreadedExecutor to enable processing goals concurrently - #executor = MultiThreadedExecutor() + # from rclpy.executors import MultiThreadedExecutor + # executor = MultiThreadedExecutor() - rclpy.spin(tf_republisher_node) #, executor) + rclpy.spin(tf_republisher_node) # , executor) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py b/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py index e2411a3..e61453f 100644 --- a/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py +++ b/tf2_web_republisher_py/tf2_web_republisher_py/tf_pair.py @@ -1,20 +1,26 @@ -from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TransformStamped, Transform +import copy + import numpy as np +from geometry_msgs.msg import Transform, TransformStamped from scipy.spatial.transform import Rotation -import math -import copy + class TFPair(object): - def __init__(self,source_frame,target_frame,angular_thres=0.0,trans_thres=0.0): + def __init__(self, source_frame, target_frame, angular_thres=0.0, trans_thres=0.0): self.is_ok = True self._source_frame = source_frame self._target_frame = target_frame self._angular_thres = angular_thres self._trans_thres = trans_thres - self._tf_transmitted = {'translation':np.array([0.0,0.0,0.0]),'rotation':Rotation.identity()} - self._tf_received = {'translation':np.array([0.0,0.0,0.0]),'rotation':Rotation.identity()} + self._tf_transmitted = { + "translation": np.array([0.0, 0.0, 0.0]), + "rotation": Rotation.identity(), + } + self._tf_received = { + "translation": np.array([0.0, 0.0, 0.0]), + "rotation": Rotation.identity(), + } self._last_tf_msg = Transform() @@ -23,7 +29,7 @@ def __init__(self,source_frame,target_frame,angular_thres=0.0,trans_thres=0.0): @property def id(self): - return self._source_frame + '-' + self._target_frame + return self._source_frame + "-" + self._target_frame @property def source_frame(self): @@ -46,31 +52,44 @@ def last_tf_msg(self): return self._last_tf_msg @source_frame.setter - def source_frame(self,value): + def source_frame(self, value): self._source_frame = value self._updated = True @target_frame.setter - def target_frame(self,value): + def target_frame(self, value): self._target_frame = value self._updated = True @angular_thres.setter - def angular_thres(self,value): + def angular_thres(self, value): self._angular_thres = value self._updated = True @trans_thres.setter - def trans_thres(self,value): + def trans_thres(self, value): self._trans_thres = value self._updated = True def transmission_triggered(self): self._tf_transmitted = copy.deepcopy(self._tf_received) - def update_transform(self,update:TransformStamped): - self._tf_received['translation'] = np.array([update.transform.translation.x,update.transform.translation.y,update.transform.translation.z]) - self._tf_received['rotation'] = Rotation.from_quat([update.transform.rotation.x, update.transform.rotation.y, update.transform.rotation.z, update.transform.rotation.w]) + def update_transform(self, update: TransformStamped): + self._tf_received["translation"] = np.array( + [ + update.transform.translation.x, + update.transform.translation.y, + update.transform.translation.z, + ] + ) + self._tf_received["rotation"] = Rotation.from_quat( + [ + update.transform.rotation.x, + update.transform.rotation.y, + update.transform.rotation.z, + update.transform.rotation.w, + ] + ) self._last_tf_msg = update.transform self._updated = True @@ -81,10 +100,16 @@ def update_needed(self): if self._trans_thres == 0.0 or self._angular_thres == 0.0: result = True self.first_transmission = False - elif self.distance(self._tf_transmitted,self._tf_received) > self._trans_thres: + elif ( + self.distance(self._tf_transmitted, self._tf_received) + > self._trans_thres + ): result = True self.first_transmission = False - elif self.angle(self._tf_transmitted,self._tf_received) > self._angular_thres: + elif ( + self.angle(self._tf_transmitted, self._tf_received) + > self._angular_thres + ): result = True self.first_transmission = False elif self.first_transmission: @@ -94,10 +119,9 @@ def update_needed(self): self._updated = False return result - @staticmethod - def distance(tf1,tf2): - return np.linalg.norm(tf1['translation']-tf2['translation']) + def distance(tf1, tf2): + return np.linalg.norm(tf1["translation"] - tf2["translation"]) @staticmethod def angle(tf1, tf2):