added static broadcaster

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>

rq_controller/tf2/static_broadcaster.py

+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

setup.py

@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'tf_static_broadcaster = rq_controller.tf2.static_broadcaster:main',
         ],
     },
 )