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..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','pyquaternion'],
+ 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 808c372..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 numpy as np
-from pyquaternion import Quaternion
-import math
import copy
+import numpy as np
+from geometry_msgs.msg import Transform, TransformStamped
+from scipy.spatial.transform import Rotation
+
+
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':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()
@@ -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'] = Quaternion(update.transform.rotation.w,update.transform.rotation.x,update.transform.rotation.y,update.transform.rotation.z)
+ 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,11 +119,13 @@ 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):
- 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())