ros小乌龟画图形c++代码

ROS是一个机器人操作系统，其中包括了一个用于创建机器人应用程序的框架。其中，小乌龟绘图是ROS中一个经典的入门例子。

以下是实现小乌龟画图形的C++代码：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
#include <iostream>

using namespace std;

const double PI = 3.14159265359;

double degree2radian(double degree)
{
    return degree * PI / 180.0;
}

class Turtle
{
public:
    Turtle()
    {
        // 初始化ROS节点
        n = ros::NodeHandle();

        // 订阅小乌龟的位置信息
        pose_sub = n.subscribe("/turtle1/pose", 10, &Turtle::poseCallback, this);

        // 发布小乌龟的速度信息
        velocity_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    }

    void move(double speed, double distance, bool isForward)
    {
        // 初始化速度消息
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = abs(speed);
        if (isForward)
        {
            vel_msg.linear.x = abs(speed);
        }
        else
        {
            vel_msg.linear.x = -abs(speed);
        }
        vel_msg.linear.y = 0;
        vel_msg.linear.z = 0;

        vel_msg.angular.x = 0;
        vel_msg.angular.y = 0;
        vel_msg.angular.z = 0;

        double t0 = ros::Time::now().toSec();
        double current_distance = 0;

        ros::Rate loop_rate(10);

        while (current_distance < distance)
        {
            velocity_pub.publish(vel_msg);
            double t1 = ros::Time::now().toSec();
            current_distance = speed * (t1 - t0);
            ros::spinOnce();
            loop_rate.sleep();
        }

        // 停止小乌龟运动
        vel_msg.linear.x = 0;
        velocity_pub.publish(vel_msg);
    }

    void rotate(double angular_speed, double angle, bool clockwise)
    {
        // 初始化速度消息
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0;
        vel_msg.linear.y = 0;
        vel_msg.linear.z = 0;

        vel_msg.angular.x = 0;
        vel_msg.angular.y = 0;
        if (clockwise)
        {
            vel_msg.angular.z = -abs(angular_speed);
        }
        else
        {
            vel_msg.angular.z = abs(angular_speed);
        }

        double current_angle = 0.0;
        double t0 = ros::Time::now().toSec();

        ros::Rate loop_rate(10);

        while (current_angle < angle)
        {
            velocity_pub.publish(vel_msg);
            double t1 = ros::Time::now().toSec();
            current_angle = angular_speed * (t1 - t0);
            ros::spinOnce();
            loop_rate.sleep();
        }

        // 停止小乌龟旋转
        vel_msg.angular.z = 0;
        velocity_pub.publish(vel_msg);
    }

    void poseCallback(const turtlesim::Pose::ConstPtr &pose_msg)
    {
        this->pose = *pose_msg;
    }

private:
    ros::NodeHandle n;
    ros::Publisher velocity_pub;
    ros::Subscriber pose_sub;
    turtlesim::Pose pose;
};

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

    Turtle turtle;

    double speed, angular_speed;
    double distance, angle;
    bool isForward, clockwise;

    cout << "请输入线速度：";
    cin >> speed;
    cout << "请输入角速度：";
    cin >> angular_speed;

    cout << "请输入距离：";
    cin >> distance;
    cout << "是否前进？（1为前进，0为后退）：";
    cin >> isForward;

    cout << "请输入旋转角度：";
    cin >> angle;
    cout << "是否顺时针旋转？（1为顺时针，0为逆时针）：";
    cin >> clockwise;

    turtle.move(speed, distance, isForward);
    turtle.rotate(degree2radian(angular_speed), degree2radian(angle), clockwise);

    // 输出小乌龟的位置信息
    ROS_INFO("Turtle position: (%.2f, %.2f)", turtle.pose.x, turtle.pose.y);
    ROS_INFO("Turtle orientation: %.2f", turtle.pose.theta);

    return 0;
}