Created
March 4, 2025 08:09
-
-
Save horverno/d9606db76aa6a6befe3df3f132d2bfcf to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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() |
Author
horverno
commented
Mar 4, 2025
Cube version:
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