Skip to content

Instantly share code, notes, and snippets.

@horverno

horverno/readme.md

Created March 4, 2025 08:09
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save horverno/d9606db76aa6a6befe3df3f132d2bfcf to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save horverno/d9606db76aa6a6befe3df3f132d2bfcf to your computer and use it in GitHub Desktop.
Download ZIP
Raw
tf_and_text_pub.py
import math
import sys
from geometry_msgs.msg import TransformStamped
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.time import Time
from rclpy.clock import Clock
from visualization_msgs.msg import Marker, MarkerArray
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
def quaternion_from_euler(ai, aj, ak):
ai /= 2.0
aj /= 2.0
ak /= 2.0
ci = math.cos(ai)
si = math.sin(ai)
cj = math.cos(aj)
sj = math.sin(aj)
ck = math.cos(ak)
sk = math.sin(ak)
cc = ci*ck
cs = ci*sk
sc = si*ck
ss = si*sk
q = np.empty((4, ))
q[0] = cj*sc - sj*cs
q[1] = cj*ss + sj*cc
q[2] = cj*cs - sj*sc
q[3] = cj*cc + sj*ss
return q
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, transformation):
super().__init__('static_turtle_tf2_broadcaster')
self.tf_static_broadcaster = StaticTransformBroadcaster(self)
# Publish static transforms once at startup
# self.make_transforms()
self.timer = self.create_timer(1.0, self.make_transforms)
self.text_pub = self.create_publisher(MarkerArray, 'text_visu', 10)
self.get_logger().info('Static TF2 broadcaster started')
def make_transforms(self):
x_range = np.arange(8.0, 10.6, 1.2)
y_range = np.arange(-5.0, 5.5, 2.5)
z_range = np.arange(0.0, 5.5, 1.5)
for x in x_range:
for y in y_range:
for z in z_range:
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'lexus3/ground_link'
t.child_frame_id = f'transform_{x}_{y}_{z}'
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = z
quat = quaternion_from_euler(0.0, 0.0, np.pi)
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]
self.tf_static_broadcaster.sendTransform(t)
# self.get_logger().info(f'Published static transform for {t.child_frame_id}')
# wait for 0.01 seconds
# Publish text markers
index = 0
index_global = 0
marker_array = MarkerArray()
for x in x_range:
marker = Marker()
marker.header.frame_id = 'lexus3/ground_link'
marker.header.stamp = self.get_clock().now().to_msg()
marker.ns = 'text'
marker.id = index_global
index_global += 1
index += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = x
marker.pose.position.y = y_range[0] - 1.0
marker.pose.position.z = z_range[0]
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.text = f'x{index}'
marker_array.markers.append(marker)
self.get_logger().info(f'Published static transform for {marker.text} at {marker.pose.position.x} {marker.pose.position.y} {marker.pose.position.z}')
self.text_pub.publish(marker_array)
index = 0
for y in y_range:
marker = Marker()
marker.header.frame_id = 'lexus3/ground_link'
marker.header.stamp = self.get_clock().now().to_msg()
marker.ns = 'text'
marker.id = index_global
index_global += 1
index += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = x_range[0] - 1.0
marker.pose.position.y = y
marker.pose.position.z = z_range[0]
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.text = f'y{index}'
marker_array.markers.append(marker)
self.get_logger().info(f'Published static transform for {marker.text} at {marker.pose.position.x} {marker.pose.position.y} {marker.pose.position.z}')
self.text_pub.publish(marker_array)
index = 0
for z in z_range:
marker = Marker()
marker.header.frame_id = 'lexus3/ground_link'
marker.header.stamp = self.get_clock().now().to_msg()
marker.ns = 'text'
marker.id = index_global
index_global += 1
index += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = x_range[-1] + 0.0
marker.pose.position.y = y_range[-1] + 0.5
marker.pose.position.z = z
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.text = f'z{index}'
marker_array.markers.append(marker)
self.get_logger().info(f'Published static transform for {marker.text} at {marker.pose.position.x} {marker.pose.position.y} {marker.pose.position.z}')
self.text_pub.publish(marker_array)
# shutdown the rclpy
# rclpy.shutdown()
def main():
print('Hello')
logger = rclpy.logging.get_logger('logger')
rclpy.init()
node = StaticFramePublisher(sys.argv)
try:
# spin once
# rclpy.spin_once(node)
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
@horverno
Copy link
Author

horverno commented Mar 4, 2025

Cube version:
image

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node
from rclpy.time import Time
from rclpy.clock import Clock
from visualization_msgs.msg import Marker, MarkerArray

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster





def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


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, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        # self.make_transforms()
        self.timer = self.create_timer(1.0, self.make_transforms)
        self.text_pub = self.create_publisher(MarkerArray, 'text_visu', 10)

        self.get_logger().info('Static TF2 broadcaster started')

    #  translation: ('0.000000', '0.000000', '-0.370000')
    # [static_transform_publisher-7] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')

    def make_transforms(self):
        ts = TransformStamped()
        ts.header.stamp = self.get_clock().now().to_msg()
        ts.header.frame_id = 'lexus3/base_link'
        ts.child_frame_id = 'lexus3/ground_link'
        ts.transform.translation.x = 0.0
        ts.transform.translation.y = 0.0
        ts.transform.translation.z = -0.37
        ts.transform.rotation.x = 0.0
        ts.transform.rotation.y = 0.0
        ts.transform.rotation.z = 0.0
        quat = quaternion_from_euler(0.0, 0.0, 0.0)
        ts.transform.rotation.x = quat[0]
        ts.transform.rotation.y = quat[1]
        ts.transform.rotation.z = quat[2]
        ts.transform.rotation.w = quat[3]
        self.tf_static_broadcaster.sendTransform(ts)
        marker_array = MarkerArray()
        x_range = np.arange(8.0, 10.6, 1.2)
        y_range = np.arange(-5.0, 5.5, 2.5)
        z_range = np.arange(0.0, 5.5, 1.5)
        for x in x_range:
            for y in y_range:
                for z in z_range:
                    marker = Marker()
                    marker.header.frame_id = 'lexus3/ground_link'
                    marker.ns = 'cube'
                    marker.id = len(marker_array.markers)
                    marker.type = Marker.CUBE
                    marker.action = Marker.ADD
                    marker.pose.position.x = x
                    marker.pose.position.y = y
                    marker.pose.position.z = z
                    quat = quaternion_from_euler(0.0, 0.0, np.pi)
                    marker.pose.orientation.x = quat[0]
                    marker.pose.orientation.y = quat[1]
                    marker.pose.orientation.z = quat[2]
                    marker.pose.orientation.w = quat[3]
                    marker.scale.x = 0.4 
                    marker.scale.y = 0.4
                    marker.scale.z = 0.4
                    marker.color.a = 1.0
                    marker.color.r = 0.2 + 0.1 * x
                    marker.color.g = 0.2 + 0.1 * y
                    marker.color.b = 0.8 - 0.1 * z
                    marker_array.markers.append(marker)
                    # t = TransformStamped()
                    # t.header.stamp = self.get_clock().now().to_msg()
                    # t.header.frame_id = 'lexus3/ground_link'
                    # t.child_frame_id = f'transform_{x}_{y}_{z}'

                    # t.transform.translation.x = x
                    # t.transform.translation.y = y
                    # t.transform.translation.z = z
                    # quat = quaternion_from_euler(0.0, 0.0, np.pi)
                    # t.transform.rotation.x = quat[0]
                    # t.transform.rotation.y = quat[1]
                    # t.transform.rotation.z = quat[2]
                    # t.transform.rotation.w = quat[3]

                    # self.tf_static_broadcaster.sendTransform(t)
                    # self.get_logger().info(f'Published static transform for {t.child_frame_id}')
                    # wait for 0.01 seconds
        # Publish text markers

        index = 0
        index_global = 0
        for x in x_range:
            marker = Marker()
            marker.header.frame_id = 'lexus3/ground_link'
            marker.header.stamp = self.get_clock().now().to_msg()
            marker.ns = 'text'
            marker.id = index_global
            index_global += 1
            index += 1
            marker.type = Marker.TEXT_VIEW_FACING
            marker.action = Marker.ADD
            marker.pose.position.x = x
            marker.pose.position.y = y_range[0] - 1.0
            marker.pose.position.z = z_range[0]
            marker.pose.orientation.x = 0.0
            marker.pose.orientation.y = 0.0
            marker.pose.orientation.z = 0.0
            marker.pose.orientation.w = 1.0
            marker.scale.z = 0.5
            marker.color.a = 1.0
            marker.color.r = 1.0
            marker.color.g = 1.0
            marker.color.b = 1.0
            marker.text = f'x{index}'
            marker_array.markers.append(marker)
            self.get_logger().info(f'Published static transform for {marker.text} at {marker.pose.position.x} {marker.pose.position.y} {marker.pose.position.z}')
        self.text_pub.publish(marker_array)
        index = 0
        for y in y_range:
            marker = Marker()
            marker.header.frame_id = 'lexus3/ground_link'
            marker.header.stamp = self.get_clock().now().to_msg()
            marker.ns = 'text'
            marker.id = index_global
            index_global += 1
            index += 1
            marker.type = Marker.TEXT_VIEW_FACING
            marker.action = Marker.ADD
            marker.pose.position.x = x_range[0] - 1.0
            marker.pose.position.y = y
            marker.pose.position.z = z_range[0]
            marker.pose.orientation.x = 0.0
            marker.pose.orientation.y = 0.0
            marker.pose.orientation.z = 0.0
            marker.pose.orientation.w = 1.0
            marker.scale.z = 0.5
            marker.color.a = 1.0
            marker.color.r = 1.0
            marker.color.g = 1.0
            marker.color.b = 1.0
            marker.text = f'y{index}'
            marker_array.markers.append(marker)
            self.get_logger().info(f'Published static transform for {marker.text} at {marker.pose.position.x} {marker.pose.position.y} {marker.pose.position.z}')
        self.text_pub.publish(marker_array)
        index = 0
        for z in z_range:
            marker = Marker()
            marker.header.frame_id = 'lexus3/ground_link'
            marker.header.stamp = self.get_clock().now().to_msg()
            marker.ns = 'text'
            marker.id = index_global
            index_global += 1
            index += 1
            marker.type = Marker.TEXT_VIEW_FACING
            marker.action = Marker.ADD
            marker.pose.position.x = x_range[-1] + 0.0
            marker.pose.position.y = y_range[-1] + 0.5
            marker.pose.position.z = z
            marker.pose.orientation.x = 0.0
            marker.pose.orientation.y = 0.0
            marker.pose.orientation.z = 0.0
            marker.pose.orientation.w = 1.0
            marker.scale.z = 0.5
            marker.color.a = 1.0
            marker.color.r = 1.0
            marker.color.g = 1.0
            marker.color.b = 1.0
            marker.text = f'z{index}'
            marker_array.markers.append(marker)
            self.get_logger().info(f'Published static transform for {marker.text} at {marker.pose.position.x} {marker.pose.position.y} {marker.pose.position.z}')
        self.text_pub.publish(marker_array)

        # shutdown the rclpy
        # rclpy.shutdown()


def main():
    print('Hello')
    logger = rclpy.logging.get_logger('logger')
    rclpy.init()
    node = StaticFramePublisher(sys.argv)
    try:
        # spin once
        # rclpy.spin_once(node)
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment