ROS_Learn/src/position_demo/scripts/subscriber_node.py

58 lines
1.4 KiB
Python
Raw Normal View History

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