#!/usr/bin/env python3

import rospy
import math
from geometry_msgs.msg import PointStamped

def main():
    rospy.init_node('Publisher_S')
    pub = rospy.Publisher('position', PointStamped, queue_size=10)    # 向 position 话题发布 PointStamped 类型的消息
    rate = rospy.Rate(10)    # 每 0.1s 一条信息

    k = 0

    while not rospy.is_shutdown():
        msg = PointStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "map"    # 基于 map 的坐标系
        msg.point.x = math.sin(0.05 * math.pi * k)    # x 值
        msg.point.y = math.cos(0.05 * math.pi * k)    # y 值
        msg.point.z = 0.0
        pub.publish(msg)    # 发布话题
        rospy.loginfo("发布端发送坐标信息: x=%.3f, y=%.3f" % (msg.point.x, msg.point.y))    # 打印坐标信息
        k += 1
        rate.sleep()

if __name__ == '__main__':
    main()