ROS C++ 编写的乌龟编队控制代码 - 保持等边三角形 - 常规

以下是基于 ROS Kinetic 版本的代码实现：

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

// 初始位置和速度
const float INIT_X = 2.0;
const float INIT_Y = 2.0;
const float INIT_VEL = 1.0;

// 控制参数
const float LINEAR_SPEED = 1.0;
const float ANGULAR_SPEED = 1.0;
const float DISTANCE = 1.0;

// 乌龟运动方向
enum Direction {
    UP,
    DOWN,
    LEFT,
    RIGHT
};

// 乌龟状态
struct TurtleState {
    float x;
    float y;
    float theta;
};

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

// 获取乌龟状态
TurtleState getTurtleState(const turtlesim::Pose& pose) {
    TurtleState state;
    state.x = pose.x;
    state.y = pose.y;
    state.theta = pose.theta;
    return state;
}

// 控制乌龟运动
void controlTurtle(ros::Publisher& pub, const TurtleState& leader, const TurtleState& follower) {
    geometry_msgs::Twist msg;
    float d = distance(leader.x, leader.y, follower.x, follower.y);
    float angle = atan2(leader.y - follower.y, leader.x - follower.x) - follower.theta;
    if (d < DISTANCE) {
        msg.linear.x = 0.0;
        msg.angular.z = 0.0;
    } else {
        msg.linear.x = LINEAR_SPEED;
        msg.angular.z = ANGULAR_SPEED * angle;
    }
    pub.publish(msg);
}

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

    // 创建乌龟A
    ros::ServiceClient spawnClient = nh.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn::Request req;
    turtlesim::Spawn::Response resp;
    req.x = INIT_X;
    req.y = INIT_Y;
    req.theta = 0.0;
    req.name = "turtleA";
    spawnClient.call(req, resp);

    // 创建乌龟B和C
    req.x = INIT_X - DISTANCE * sqrt(3) / 2;
    req.y = INIT_Y + DISTANCE / 2;
    req.theta = M_PI / 3;
    req.name = "turtleB";
    spawnClient.call(req, resp);
    req.x = INIT_X + DISTANCE * sqrt(3) / 2;
    req.y = INIT_Y + DISTANCE / 2;
    req.theta = -M_PI / 3;
    req.name = "turtleC";
    spawnClient.call(req, resp);

    // 获取乌龟A、B、C的控制器
    ros::Publisher pubA = nh.advertise<geometry_msgs::Twist>("/turtleA/cmd_vel", 1);
    ros::Publisher pubB = nh.advertise<geometry_msgs::Twist>("/turtleB/cmd_vel", 1);
    ros::Publisher pubC = nh.advertise<geometry_msgs::Twist>("/turtleC/cmd_vel", 1);

    // 获取乌龟A、B、C的状态
    ros::Subscriber subA = nh.subscribe("/turtleA/pose", 1, [&](const turtlesim::Pose& pose) {
        TurtleState state = getTurtleState(pose);
        controlTurtle(pubB, state, getTurtleState(turtlesim::Pose()));
        controlTurtle(pubC, state, getTurtleState(turtlesim::Pose()));
    });
    ros::Subscriber subB = nh.subscribe("/turtleB/pose", 1, [&](const turtlesim::Pose& pose) {
        TurtleState state = getTurtleState(pose);
        controlTurtle(pubA, state, getTurtleState(turtlesim::Pose()));
        controlTurtle(pubC, state, getTurtleState(turtlesim::Pose()));
    });
    ros::Subscriber subC = nh.subscribe("/turtleC/pose", 1, [&](const turtlesim::Pose& pose) {
        TurtleState state = getTurtleState(pose);
        controlTurtle(pubA, state, getTurtleState(turtlesim::Pose()));
        controlTurtle(pubB, state, getTurtleState(turtlesim::Pose()));
    });

    // 循环执行ROS节点
    ros::spin();

    return 0;
}

代码解析：

首先，我们定义了三个常量，分别表示乌龟的初始位置和速度、控制参数以及编队距离。
然后，我们定义了一个枚举类型 Direction，表示乌龟的运动方向。
接着，我们定义了一个结构体 TurtleState，表示乌龟的状态，包括位置和朝向。
我们定义了一个函数 distance，用于计算两点之间的距离。
我们定义了一个函数 getTurtleState，用于获取乌龟的状态。
我们定义了一个函数 controlTurtle，用于控制乌龟的运动。该函数接受三个参数，分别为领航乌龟的状态、跟随乌龟的状态以及控制器。该函数首先计算跟随乌龟和领航乌龟之间的距离和角度差，然后根据控制参数计算出线速度和角速度，最后发布控制指令。
在主函数中，我们首先初始化ROS节点，并创建三只乌龟A、B、C。乌龟A的初始位置为(2, 2)，乌龟B和C的初始位置分别为A的左右两侧，距离为1，且朝向一致。
然后，我们获取三只乌龟的控制器和状态，并分别订阅它们的位姿信息。当接收到位姿信息时，我们根据乌龟的状态和领航乌龟的状态计算出控制指令，并发布给跟随乌龟的控制器。
最后，我们循环执行ROS节点。