58 lines
1.4 KiB
Python
Executable File
58 lines
1.4 KiB
Python
Executable File
#!/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()
|