ROS CPP 编写的乌龟等边三角形编队控制代码

以下是用 ROS CPP 编写的代码，实现了两只乌龟 A、B、C 始终保持等边三角形编队的功能，键盘控制乌龟 A 运动，A、B、C 会始终保持等边三角形编队，边长为 1 且朝向一致。

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

const double EDGE_LENGTH = 1.0;

class TurtleFormation
{
public:
    TurtleFormation()
    {
        // 初始化 ROS 节点
        ros::NodeHandle nh;

        // 创建乌龟 A
        spawnTurtle('turtleA', 1, 1, 0);

        // 创建乌龟 B
        spawnTurtle('turtleB', 1 - EDGE_LENGTH / 2, 1 + EDGE_LENGTH * sqrt(3) / 2, M_PI / 3);

        // 创建乌龟 C
        spawnTurtle('turtleC', 1 + EDGE_LENGTH / 2, 1 + EDGE_LENGTH * sqrt(3) / 2, -M_PI / 3);

        // 订阅乌龟 A 的速度控制命令
        sub_ = nh.subscribe('/turtleA/cmd_vel', 10, &TurtleFormation::velCallback, this);

        // 发布乌龟 B 和 C 的速度控制命令
        pubB_ = nh.advertise<geometry_msgs::Twist>('/turtleB/cmd_vel', 10);
        pubC_ = nh.advertise<geometry_msgs::Twist>('/turtleC/cmd_vel', 10);
    }

private:
    ros::Subscriber sub_;
    ros::Publisher pubB_;
    ros::Publisher pubC_;

    void spawnTurtle(const std::string& name, double x, double y, double theta)
    {
        // 创建服务客户端
        ros::NodeHandle nh;
        ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>('/spawn');

        // 创建请求消息
        turtlesim::Spawn srv;
        srv.request.x = x;
        srv.request.y = y;
        srv.request.theta = theta;
        srv.request.name = name;

        // 调用服务
        if (client.call(srv))
        {
            ROS_INFO('Spawned turtle %s', srv.response.name.c_str());
        }
        else
        {
            ROS_ERROR('Failed to spawn turtle %s', name.c_str());
        }
    }

    void velCallback(const geometry_msgs::Twist::ConstPtr& msg)
    {
        // 发布速度控制命令
        pubB_.publish(msg);
        pubC_.publish(msg);
    }
};

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

    // 创建 TurtleFormation 对象
    TurtleFormation formation;

    // 进入 ROS 循环
    ros::spin();

    return 0;
}

代码解释：

1. 定义了常量 EDGE_LENGTH，表示等边三角形的边长为 1。
2. 定义了 TurtleFormation 类，用于管理三只乌龟的运动。
3. 在 TurtleFormation 的构造函数中，首先创建了三只乌龟 A、B、C，并分别设置它们的初始位置和朝向。
4. 订阅了乌龟 A 的速度控制命令，当乌龟 A 接收到速度控制命令时，会将该命令发送给乌龟 B 和 C。
5. 发布了乌龟 B 和 C 的速度控制命令，使它们始终保持与乌龟 A 等边三角形的编队。
6. 在 main 函数中，创建了 TurtleFormation 对象，并进入 ROS 循环。

运行代码：

1. 在终端中打开 ROS 核心节点：roscore
2. 在另一个终端中运行该节点：rosrun turtle_formation turtle_formation
3. 在 RViz 中查看乌龟的运动：rosrun rviz rviz，在 RViz 中添加 Turtle 和 TF 两个显示项，即可看到三只乌龟始终保持等边三角形的编队。
4. 在 RViz 中按下键盘上的方向键，即可控制乌龟 A 的运动。