ROS----小乌龟画爱心

turtle走爱心形状

代码如下

#include "ros/ros.h"
#include

int main(int argc, char *argv[])
{
    double PI = 3.141592653589793653589793;
    ros::init(argc, argv, "heart_shape");
    ros::NodeHandle n;
    ros::Publisher vel_pub = n.advertise("/turtle1/cmd_vel", 10);
    geometry_msgs::Twist vel_cmd;
    ros::Rate loopRate(2);
    int count = 0;
    int i = 0;
    while (ros::ok())
    {
        geometry_msgs::Twist msg;
        msg.linear.x = 1;
        msg.angular.z = 1;
        i++; 

        if (i ==9 )
        {

            msg.linear.x = 0;
            msg.angular.z = 2*PI;
        }
        
        if (i == 16)
        {
            ;
            msg.linear.x = 1.0;
            msg.angular.z = 2;
        }

        if (i >= 17)
        {
            ;
            msg.linear.x = 1;
            msg.angular.z = 0;
            i++;
            if (i >= 27)
            {    
                msg.linear.x = 0.15;
                msg.linear.y = 1;
                i++;
                if (i >= 43)
                {
                    msg.linear.x = 0;
                    msg.linear.y = 0;
            }
        }
            
            
        }

        vel_pub.publish(msg);

        //发布消息
        ROS_INFO_STREAM("Sending random velocity command: "
                        << "linear = " << msg.linear.x
                        << " angular = " << msg.angular.z);
        //按照循环频率延时
        //ros::spinOnce();
        loopRate.sleep();
    }

        return 0;
}