#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PointStamped, Point
from visualization_msgs.msg import Marker, MarkerArray
from std_msgs.msg import ColorRGBA

MAX_POINTS = 100

original_trajectory = []
marker_pub = None

def position_callback(msg):
    global original_trajectory
    global marker_pub

    original_trajectory.append(msg.point)
    if len(original_trajectory) > MAX_POINTS:
        original_trajectory.pop(0)
    
    marker_array = MarkerArray()
    original_line = Marker()
    original_line.header = msg.header
    original_line.ns = "trajectory"
    original_line.id = 0
    original_line.type = Marker.LINE_STRIP
    original_line.action = Marker.ADD
    original_line.scale.x = 0.02
    original_line.color.g = 1.0
    original_line.color.a = 1.0
    original_line.points = list(original_trajectory)
    marker_array.markers.append(original_line)

    marker = Marker()
    marker.header.frame_id = "map"    # 设置坐标系
    marker.header.stamp = rospy.Time.now()
    marker.type = Marker.POINTS
    marker.action = Marker.ADD
    marker.scale.x = 0.025    # 点的直径
    marker.scale.y = 0.025    # 点的直径
    marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)    # 红色

    marker.points = [Point(x=p.x, y=p.y, z=p.z) for p in original_trajectory]
    marker_array.markers.append(marker)

    marker_pub.publish(marker_array)
    rospy.loginfo("订阅端接收坐标信息: x=%.3f, y=%.3f ",msg.point.x, msg.point.y)

if __name__ == '__main__':
    rospy.init_node('Subscriber_Client_S')

    rospy.Subscriber('position', PointStamped, position_callback)

    marker_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)

    rospy.loginfo("订阅端节点启动成功，等待发布端信息。")
    rospy.spin()