ROS C++ 代码实现乌龟编队：等边三角形运动

以下是实现该功能的 ROS C++ 代码：

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

// 三只乌龟的全局变量
turtlesim::Pose turtleA_pose;
turtlesim::Pose turtleB_pose;
turtlesim::Pose turtleC_pose;

// 计算两点之间的距离
float distance(float x1, float y1, float x2, float y2)
{
    return std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2));
}

// 计算两点之间的角度
float angle(float x1, float y1, float x2, float y2)
{
    return std::atan2(y2 - y1, x2 - x1);
}

// 乌龟A的回调函数
void turtleA_callback(const turtlesim::Pose::ConstPtr& pose_msg)
{
    turtleA_pose = *pose_msg;

    // 计算乌龟B和乌龟C的位置
    float xB = turtleA_pose.x + std::cos(turtleA_pose.theta + M_PI / 3);
    float yB = turtleA_pose.y + std::sin(turtleA_pose.theta + M_PI / 3);
    float xC = turtleA_pose.x + std::cos(turtleA_pose.theta - M_PI / 3);
    float yC = turtleA_pose.y + std::sin(turtleA_pose.theta - M_PI / 3);

    // 发布速度控制指令
    ros::NodeHandle nh;
    ros::Publisher turtleB_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
    ros::Publisher turtleC_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 10);
    geometry_msgs::Twist vel_msg;

    // 计算乌龟B和乌龟C需要前进的距离和旋转的角度
    float distanceB = distance(turtleB_pose.x, turtleB_pose.y, xB, yB);
    float distanceC = distance(turtleC_pose.x, turtleC_pose.y, xC, yC);
    float angleB = angle(turtleB_pose.x, turtleB_pose.y, xB, yB) - turtleB_pose.theta;
    float angleC = angle(turtleC_pose.x, turtleC_pose.y, xC, yC) - turtleC_pose.theta;

    // 发布速度控制指令使乌龟B和乌龟C前进和旋转
    vel_msg.linear.x = distanceB;
    vel_msg.angular.z = angleB;
    turtleB_vel_pub.publish(vel_msg);

    vel_msg.linear.x = distanceC;
    vel_msg.angular.z = angleC;
    turtleC_vel_pub.publish(vel_msg);
}

// 乌龟B的回调函数
void turtleB_callback(const turtlesim::Pose::ConstPtr& pose_msg)
{
    turtleB_pose = *pose_msg;
}

// 乌龟C的回调函数
void turtleC_callback(const turtlesim::Pose::ConstPtr& pose_msg)
{
    turtleC_pose = *pose_msg;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtle_triangle");
    ros::NodeHandle nh;

    // 订阅乌龟A、B、C的位置信息
    ros::Subscriber turtleA_sub = nh.subscribe("/turtle1/pose", 10, turtleA_callback);
    ros::Subscriber turtleB_sub = nh.subscribe("/turtle2/pose", 10, turtleB_callback);
    ros::Subscriber turtleC_sub = nh.subscribe("/turtle3/pose", 10, turtleC_callback);

    // 控制乌龟A运动
    ros::Publisher turtleA_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    geometry_msgs::Twist vel_msg;

    while (ros::ok())
    {
        // 读取键盘输入
        char c;
        std::cout << "Enter a key: ";
        std::cin >> c;

        // 根据键盘输入计算速度控制指令
        switch (c)
        {
        case 'w':
            vel_msg.linear.x = 1;
            vel_msg.angular.z = 0;
            break;
        case 'a':
            vel_msg.linear.x = 0;
            vel_msg.angular.z = 1;
            break;
        case 's':
            vel_msg.linear.x = -1;
            vel_msg.angular.z = 0;
            break;
        case 'd':
            vel_msg.linear.x = 0;
            vel_msg.angular.z = -1;
            break;
        default:
            vel_msg.linear.x = 0;
            vel_msg.angular.z = 0;
            break;
        }

        // 发布速度控制指令
        turtleA_vel_pub.publish(vel_msg);

        // 循环等待回调函数
        ros::spinOnce();
    }

    return 0;
}

该代码中，我们订阅了三只乌龟的位置信息，并在乌龟A的回调函数中计算出乌龟B和乌龟C需要前进和旋转的距离和角度，然后通过发布速度控制指令使乌龟B和乌龟C移动和旋转。在主循环中，我们通过读取键盘输入来控制乌龟A的运动，并通过发布速度控制指令来实现。