2025-06-28 15:31:08 +08:00
|
|
|
|
#!/usr/bin/env python3
|
|
|
|
|
import rospy
|
|
|
|
|
from geometry_msgs.msg import Point
|
|
|
|
|
from visualization_msgs.msg import Marker
|
|
|
|
|
from std_msgs.msg import ColorRGBA
|
2025-06-28 22:03:23 +08:00
|
|
|
|
|
2025-06-28 15:31:08 +08:00
|
|
|
|
# 保存接收到的所有点,更新轨迹
|
|
|
|
|
points = []
|
2025-06-28 22:03:23 +08:00
|
|
|
|
|
2025-06-28 15:31:08 +08:00
|
|
|
|
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()
|
|
|
|
|
|
2025-06-28 22:03:23 +08:00
|
|
|
|
# 设置 Marker 类型为点(而不是线条)
|
|
|
|
|
marker.type = Marker.POINTS
|
2025-06-28 15:31:08 +08:00
|
|
|
|
marker.action = Marker.ADD
|
|
|
|
|
|
2025-06-28 22:03:23 +08:00
|
|
|
|
# 设置点的大小,直径为 0.001
|
|
|
|
|
marker.scale.x = 0.025 # 点的直径(x 和 y 均为直径)
|
|
|
|
|
marker.scale.y = 0.025 # 点的直径
|
|
|
|
|
|
|
|
|
|
# 设置点的颜色
|
2025-06-28 15:31:08 +08:00
|
|
|
|
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)
|
|
|
|
|
|
2025-06-28 22:03:23 +08:00
|
|
|
|
# 将所有的点添加到 Marker 的点集合中
|
2025-06-28 15:31:08 +08:00
|
|
|
|
marker.points = points
|
|
|
|
|
|
|
|
|
|
# 发布 Marker
|
|
|
|
|
marker_pub.publish(marker)
|
2025-06-28 22:03:23 +08:00
|
|
|
|
|
2025-06-28 15:31:08 +08:00
|
|
|
|
def listener():
|
|
|
|
|
rospy.init_node('position_subscriber', anonymous=True)
|
|
|
|
|
|
|
|
|
|
# 创建 Marker 发布者
|
|
|
|
|
global marker_pub
|
|
|
|
|
marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
|
2025-06-28 22:03:23 +08:00
|
|
|
|
|
2025-06-28 15:31:08 +08:00
|
|
|
|
rospy.Subscriber('position_topic', Point, callback)
|
2025-06-28 22:03:23 +08:00
|
|
|
|
|
2025-06-28 15:31:08 +08:00
|
|
|
|
rospy.spin()
|
2025-06-28 22:03:23 +08:00
|
|
|
|
|
2025-06-28 15:31:08 +08:00
|
|
|
|
if __name__ == '__main__':
|
|
|
|
|
listener()
|