#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include <cmath>

int main(int argc, char **argv) {
    ros::init(argc, argv, "nodeA");
    ros::NodeHandle nh;
    
    // 发布位置数据
    ros::Publisher pos_pub = nh.advertise<geometry_msgs::Point>("position_topic", 10);
    
    // 发布可视化标记
    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
    
    ros::Rate loop_rate(10); // 10Hz = 100ms
    int k = 0;
    
    // 创建线形标记用于显示轨迹
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "trajectory";
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = 1;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_strip.scale.x = 0.05; // 线宽
    line_strip.color.r = 1.0; // 红色
    line_strip.color.g = 0.0;
    line_strip.color.b = 0.0;
    line_strip.color.a = 1.0; // 不透明

    while (ros::ok()) {
        geometry_msgs::Point position;
        position.x = sin(0.05 * M_PI * k);
        position.y = cos(0.05 * M_PI * k);
        position.z = 0; // 2D平面运动
        
        // 发布位置数据
        pos_pub.publish(position);
        ROS_INFO("Published position: (%.2f, %.2f)", position.x, position.y);
        
        // 添加点到轨迹
        line_strip.points.push_back(position);
        
        // 更新标记时间戳
        line_strip.header.stamp = ros::Time::now();
        
        // 发布轨迹标记
        marker_pub.publish(line_strip);
        
        k++;
        loop_rate.sleep();
    }
    return 0;
}