#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <turtlesim/TeleportAbsolute.h>
#include <turtlesim/Pose.h>
#include "math.h"
// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc , char **argv){
    //ros节点初始化
    ros::init(argc,argv,"velocity_publisher");

    //创建节点句柄
    ros::NodeHandle n;
    ros::service::waitForService("/turtle1/teleport_absolute");
    //创建一个publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist,队列长度10
    ros::ServiceClient turtle_vel_pub = n.serviceClient<turtlesim::TeleportAbsolute>("/turtle1/teleport_absolute");
    ros::Subscriber subscriber = n.subscribe<turtlesim::Pose>("/turtle1/pose", 10, poseCallback);
    //初始化geometry_msgs::Twist类型的消息
    turtlesim::TeleportAbsolute vel_msg;
    turtlesim::Pose pose;
    int i=0;
    float a=0;
    for(i=0;i<=100;i++){
        vel_msg.request.x =(16*pow(sin(a),3))/5+5.54;
        vel_msg.request.y = (13*cos(a)-5*cos(2*a)-2*cos(3*a)-cos(4*a))/5+5.54;
        vel_msg.request.theta =0;
        turtle_vel_pub.call(vel_msg);
        a=a+0.0628;
      sleep(0.1);
    }

    return 0;
}