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()