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

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

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

// 定义全局变量
ros::Publisher turtleA_pub;
ros::Publisher turtleB_pub;
ros::Publisher turtleC_pub;
turtlesim::Pose turtleA_pose;
turtlesim::Pose turtleB_pose;
turtlesim::Pose turtleC_pose;

// 回调函数，获取A乌龟的位置
void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    turtleA_pose = *msg;
}

// 回调函数，获取B乌龟的位置
void turtleBPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    turtleB_pose = *msg;
}

// 回调函数，获取C乌龟的位置
void turtleCPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    turtleC_pose = *msg;
}

// 发布速度控制消息
void publishVelocity(ros::Publisher& pub, float linear, float angular)
{
    geometry_msgs::Twist msg;
    msg.linear.x = linear;
    msg.angular.z = angular;
    pub.publish(msg);
}

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

    // 创建三只乌龟
    ros::ServiceClient spawn_client = nh.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn spawn_srv;
    spawn_srv.request.x = 5.0;
    spawn_srv.request.y = 5.0;
    spawn_srv.request.theta = 0.0;
    spawn_srv.request.name = "turtleA";
    spawn_client.call(spawn_srv);
    spawn_srv.request.x = 6.0;
    spawn_srv.request.y = 5.0;
    spawn_srv.request.theta = 0.0;
    spawn_srv.request.name = "turtleB";
    spawn_client.call(spawn_srv);
    spawn_srv.request.x = 5.5;
    spawn_srv.request.y = 6.0;
    spawn_srv.request.theta = 0.0;
    spawn_srv.request.name = "turtleC";
    spawn_client.call(spawn_srv);

    // 获取三只乌龟的初始位置
    ros::Subscriber turtleA_pose_sub = nh.subscribe("/turtleA/pose", 1, turtleAPoseCallback);
    ros::Subscriber turtleB_pose_sub = nh.subscribe("/turtleB/pose", 1, turtleBPoseCallback);
    ros::Subscriber turtleC_pose_sub = nh.subscribe("/turtleC/pose", 1, turtleCPoseCallback);
    while (ros::ok() && (turtleA_pose.x == 0 || turtleB_pose.x == 0 || turtleC_pose.x == 0))
    {
        ros::spinOnce();
    }

    // 获取三只乌龟的初始方向
    float direction = turtleA_pose.theta;

    // 创建三只乌龟的速度控制发布器
    turtleA_pub = nh.advertise<geometry_msgs::Twist>("/turtleA/cmd_vel", 1);
    turtleB_pub = nh.advertise<geometry_msgs::Twist>("/turtleB/cmd_vel", 1);
    turtleC_pub = nh.advertise<geometry_msgs::Twist>("/turtleC/cmd_vel", 1);

    // 进入键盘控制循环
    ros::Rate rate(10);
    while (ros::ok())
    {
        // 获取键盘输入
        char c;
        std::cout << "Enter a key to control turtleA (w: forward, a: turn left, d: turn right): ";
        std::cin >> c;

        // 根据键盘输入控制A乌龟移动
        if (c == 'w')
        {
            publishVelocity(turtleA_pub, 1.0, 0.0);
        }
        else if (c == 'a')
        {
            publishVelocity(turtleA_pub, 0.0, 1.0);
        }
        else if (c == 'd')
        {
            publishVelocity(turtleA_pub, 0.0, -1.0);
        }
        else
        {
            publishVelocity(turtleA_pub, 0.0, 0.0);
        }

        // 计算B、C乌龟应该移动的位置和方向
        float distance = 1.0;
        float angle = M_PI / 3.0;
        float xB = turtleA_pose.x + distance * cos(direction + angle);
        float yB = turtleA_pose.y + distance * sin(direction + angle);
        float xC = turtleA_pose.x + distance * cos(direction - angle);
        float yC = turtleA_pose.y + distance * sin(direction - angle);
        float directionB = direction + angle;
        float directionC = direction - angle;

        // 计算B、C乌龟需要移动的线速度和角速度
        float linearB = sqrt(pow(xB - turtleB_pose.x, 2) + pow(yB - turtleB_pose.y, 2));
        float angularB = atan2(yB - turtleB_pose.y, xB - turtleB_pose.x) - turtleB_pose.theta;
        float linearC = sqrt(pow(xC - turtleC_pose.x, 2) + pow(yC - turtleC_pose.y, 2));
        float angularC = atan2(yC - turtleC_pose.y, xC - turtleC_pose.x) - turtleC_pose.theta;

        // 发布B、C乌龟的速度控制消息
        publishVelocity(turtleB_pub, linearB, angularB);
        publishVelocity(turtleC_pub, linearC, angularC);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

该代码中，我们首先创建了三只乌龟 A、B、C，并获取它们的初始位置和方向。然后进入键盘控制循环，根据键盘输入控制 A 乌龟的移动，并计算 B、C 乌龟应该移动的位置和方向。最后，根据 B、C 乌龟与目标位置的距离和角度计算它们需要移动的线速度和角速度，并发布速度控制消息。

需要注意的是，在计算 B、C 乌龟的位置和方向时，我们假设它们与 A 乌龟始终保持等边三角形的编队，即 B 乌龟与 A 乌龟的方向相差 60 度，C 乌龟与 A 乌龟的方向相差 -60 度。在计算 B、C 乌龟需要移动的线速度和角速度时，我们分别计算它们与目标位置的距离和角度，并使用 PID 控制器控制它们的移动。

使用方法：

启动 Turtlesim：roslaunch turtlesim turtlesim_node
运行代码：rosrun [your_package_name] turtle_triangle
使用键盘控制 A 乌龟移动，观察 B、C 乌龟的运动，保持编队形态。

代码解析：

创建三只乌龟： 使用 ros::ServiceClient 和 turtlesim::Spawn 服务创建三只乌龟 A、B、C，并设置它们的初始位置和方向。
获取乌龟位置： 使用 ros::Subscriber 订阅 /turtleA/pose、/turtleB/pose、/turtleC/pose 主题，获取三只乌龟的实时位置信息。
键盘控制： 使用 std::cin 获取键盘输入，根据输入控制 A 乌龟移动。
计算编队位置： 根据 A 乌龟的位置和方向，计算 B、C 乌龟应该移动到的位置，保持等边三角形编队。
发布速度控制消息： 使用 ros::Publisher 发布速度控制消息到 /turtleA/cmd_vel、/turtleB/cmd_vel、/turtleC/cmd_vel 主题，控制三只乌龟的运动。

注意：

该代码使用简单的几何计算来控制乌龟运动，没有使用 PID 控制等更复杂的控制策略。
可以根据需要调整代码中的参数，例如三角形的边长、乌龟移动速度等。
该代码仅供参考，可以根据实际需求进行修改和完善。