ROSc++实现小乌龟做五边形运动，并输出位姿

好的，以下是实现小乌龟做五边形运动并输出位姿的ROS C++代码：

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

// 定义全局变量表示小乌龟的位置和朝向角度
float turtle_x = 0.0;
float turtle_y = 0.0;
float turtle_theta = 0.0;

void poseCallback(const turtlesim::Pose::ConstPtr& pose)
{
    // 更新小乌龟的位置和朝向角度
    turtle_x = pose->x;
    turtle_y = pose->y;
    turtle_theta = pose->theta;
}

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

    // 创建节点句柄
    ros::NodeHandle nh;

    // 创建小乌龟速度发布者
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 创建小乌龟位姿订阅者
    ros::Subscriber pose_sub = nh.subscribe("/turtle1/pose", 10, poseCallback);

    // 休眠1秒等待ROS节点初始化完成
    ros::Duration(1).sleep();

    // 创建Twist消息并设置线速度和角速度
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 1.0;
    vel_msg.angular.z = 72.0 / 180.0 * 3.1416;

    // 计算五边形需要移动的时间
    float time = 100.0 / vel_msg.linear.x;

    // 获取当前时间
    ros::Time start_time = ros::Time::now();

    // 循环移动小乌龟
    while (ros::Time::now() - start_time < ros::Duration(time))
    {
        vel_pub.publish(vel_msg);
        ros::spinOnce();
    }

    // 停止小乌龟运动
    vel_msg.linear.x = 0.0;
    vel_msg.angular.z = 0.0;
    vel_pub.publish(vel_msg);

    // 输出小乌龟的位置和朝向角度
    ROS_INFO("位姿：x=%f, y=%f, theta=%f", turtle_x, turtle_y, turtle_theta);

    return 0;
}

运行代码后，小乌龟会在ROS环境下画出一个五边形，并输出小乌龟的位置和朝向角度。注意需要在另一个终端中运行`roscore`命令开启ROS环境。