#!/usr/bin/env python3 import rospy from geometry_msgs.msg import Point from visualization_msgs.msg import Marker from std_msgs.msg import ColorRGBA # 保存接收到的所有点,更新轨迹 points = [] def callback(data): global points # 打印接收到的坐标点 rospy.loginfo(f"Received Point: x={data.x:.4f}, y={data.y:.4f}") # 创建 Marker 消息 marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.Time.now() # 设置 Marker 类型为点(而不是线条) marker.type = Marker.POINTS marker.action = Marker.ADD # 设置点的大小,直径为 0.001 marker.scale.x = 0.025 # 点的直径(x 和 y 均为直径) marker.scale.y = 0.025 # 点的直径 # 设置点的颜色 marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0) # 红色,透明度 1.0 # 添加接收到的坐标点 point = Point() point.x = data.x point.y = data.y point.z = data.z points.append(point) # 将所有的点添加到 Marker 的点集合中 marker.points = points # 发布 Marker marker_pub.publish(marker) def listener(): rospy.init_node('position_subscriber', anonymous=True) # 创建 Marker 发布者 global marker_pub marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) rospy.Subscriber('position_topic', Point, callback) rospy.spin() if __name__ == '__main__': listener()