#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <cmath>

int main(int argc, char** argv) {
    ros::init(argc, argv, "nodeA");
    ros::NodeHandle nh;
    
    ros::Publisher pub = nh.advertise<geometry_msgs::PointStamped>("position", 10);
    
    ros::Rate rate(10); // 10Hz
    int k = 0;
    
    while (ros::ok()) {
        geometry_msgs::PointStamped msg;
        
        // 添加时间戳和坐标系
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "map"; // 重要：设置坐标系
        
        // 计算位置
        msg.point.x = std::sin(0.05 * M_PI * k);
        msg.point.y = std::cos(0.05 * M_PI * k);
        msg.point.z = 0.0;
        
        pub.publish(msg);
        ROS_INFO("Published: x=%.3f, y=%.3f", msg.point.x, msg.point.y);
        
        k++;
        rate.sleep();
    }
    
    return 0;
}