From 2159d22967ea8fa7f8dbeb58c95e9e00ac0790b5 Mon Sep 17 00:00:00 2001
From: swittl <simon.wittl@th-deg.de>
Date: Wed, 10 Jul 2024 09:24:33 +0200
Subject: [PATCH] added static broadcaster
---
package.xml | 6 ++
rq_controller/tf2/__init__.py | 0
rq_controller/tf2/static_broadcaster.py | 73 +++++++++++++++++++++++++
setup.py | 1 +
4 files changed, 80 insertions(+)
create mode 100644 rq_controller/tf2/__init__.py
create mode 100644 rq_controller/tf2/static_broadcaster.py
diff --git a/package.xml b/package.xml
index fd22eac..9ede545 100644
--- a/package.xml
+++ b/package.xml
@@ -12,6 +12,12 @@
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>python3-numpy</exec_depend>
+ <exec_depend>rclpy</exec_depend>
+ <exec_depend>tf2_ros_py</exec_depend>
+ <exec_depend>turtlesim</exec_depend>
+
<export>
<build_type>ament_python</build_type>
</export>
diff --git a/rq_controller/tf2/__init__.py b/rq_controller/tf2/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/rq_controller/tf2/static_broadcaster.py b/rq_controller/tf2/static_broadcaster.py
new file mode 100644
index 0000000..39f5cd9
--- /dev/null
+++ b/rq_controller/tf2/static_broadcaster.py
@@ -0,0 +1,73 @@
+import math
+import sys
+
+from geometry_msgs.msg import TransformStamped
+
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
+
+
+class StaticFramePublisher(Node):
+ """
+ Broadcast transforms that never change.
+
+ This example publishes transforms from `world` to a static turtle frame.
+ The transforms are only published once at startup, and are constant for all
+ time.
+ """
+
+ def __init__(self, frame, position, quaternion, name: str = 'static_broadcaster', parent_frame: str = 'world'):
+ super().__init__(name)
+
+ self.tf_static_broadcaster = StaticTransformBroadcaster(self)
+
+ # Publish static transforms once at startup
+ self.make_transforms(frame, position.split(" "), quaternion.split(" "), parent_frame)
+
+ def make_transforms(self, frame, position, quaternion, parent_frame):
+ t = TransformStamped()
+
+ t.header.stamp = self.get_clock().now().to_msg()
+ t.header.frame_id = parent_frame
+ t.child_frame_id = frame
+
+ t.transform.translation.x = float(position[0])
+ t.transform.translation.y = float(position[1])
+ t.transform.translation.z = float(position[2])
+ t.transform.rotation.x = float(quaternion[0])
+ t.transform.rotation.y = float(quaternion[1])
+ t.transform.rotation.z = float(quaternion[2])
+ t.transform.rotation.w = float(quaternion[3])
+
+ self.tf_static_broadcaster.sendTransform(t)
+
+
+def main(args=None):
+ logger = rclpy.logging.get_logger('logger')
+
+ rclpy.init(args=args)
+
+ node = rclpy.create_node('static_broadcaster')
+ parent_frame = node.declare_parameter('parent_frame', 'world').value
+ frame = node.declare_parameter('frame', 'object').value
+ name = node.declare_parameter('name', 'static_broadcaster').value
+ position = node.declare_parameter('position', '0 0 0').value
+ quaternion = node.declare_parameter('quaternion', '0 0 0 1').value
+
+ node = StaticFramePublisher(
+ frame,
+ position,
+ quaternion,
+ name,
+ parent_frame)
+
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+
+ rclpy.shutdown()
\ No newline at end of file
diff --git a/setup.py b/setup.py
index 9961d05..4ba1f90 100644
--- a/setup.py
+++ b/setup.py
@@ -20,6 +20,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
+ 'tf_static_broadcaster = rq_controller.tf2.static_broadcaster:main',
],
},
)
--
GitLab