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

以下是一个可能的实现：

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

const double SIDE_LENGTH = 1.0; // 边长
const double ANGULAR_VEL = 1.0; // 角速度

class TurtleController {
public:
    TurtleController(const std::string& name, const std::string& cmd_topic)
        : name_(name), cmd_topic_(cmd_topic)
    {
        // 订阅键盘控制指令
        cmd_sub_ = nh_.subscribe<geometry_msgs::Twist>(cmd_topic_, 10, &TurtleController::cmdCallback, this);

        // 发布运动指令
        vel_pub_ = nh_.advertise<geometry_msgs::Twist>(name_ + "/cmd_vel", 10);
    }

private:
    void cmdCallback(const geometry_msgs::Twist::ConstPtr& msg)
    {
        // 将键盘控制指令转换为运动指令
        geometry_msgs::Twist vel;
        vel.linear.x = msg->linear.x;
        vel.angular.z = msg->angular.z;

        // 发布运动指令
        vel_pub_.publish(vel);
    }

    std::string name_;
    std::string cmd_topic_;
    ros::NodeHandle nh_;
    ros::Subscriber cmd_sub_;
    ros::Publisher vel_pub_;
};

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

    // 创建三只乌龟
    TurtleController turtle_a("turtle_a", "/turtle_a/cmd_vel");
    TurtleController turtle_b("turtle_b", "/turtle_b/cmd_vel");
    TurtleController turtle_c("turtle_c", "/turtle_c/cmd_vel");

    // 循环发布运动指令
    ros::Rate rate(10);
    while (ros::ok()) {
        // 计算三只乌龟的位置
        double x_a, y_a, theta_a;
        nh.getParam("turtle_a/x", x_a);
        nh.getParam("turtle_a/y", y_a);
        nh.getParam("turtle_a/theta", theta_a);

        double x_b = x_a + SIDE_LENGTH * cos(theta_a);
        double y_b = y_a + SIDE_LENGTH * sin(theta_a);
        double theta_b = theta_a;

        double x_c = x_a + SIDE_LENGTH * cos(theta_a + 2 * M_PI / 3);
        double y_c = y_a + SIDE_LENGTH * sin(theta_a + 2 * M_PI / 3);
        double theta_c = theta_a + 2 * M_PI / 3;

        // 发布运动指令
        geometry_msgs::Twist vel_a, vel_b, vel_c;
        vel_a.linear.x = 1.0;
        vel_a.angular.z = 0.0;

        double dx_b = x_b - x_a;
        double dy_b = y_b - y_a;
        double dtheta_b = atan2(dy_b, dx_b) - theta_a;
        if (dtheta_b > M_PI) dtheta_b -= 2 * M_PI;
        if (dtheta_b < -M_PI) dtheta_b += 2 * M_PI;
        vel_b.linear.x = sqrt(dx_b * dx_b + dy_b * dy_b);
        vel_b.angular.z = ANGULAR_VEL * dtheta_b;

        double dx_c = x_c - x_a;
        double dy_c = y_c - y_a;
        double dtheta_c = atan2(dy_c, dx_c) - theta_a;
        if (dtheta_c > M_PI) dtheta_c -= 2 * M_PI;
        if (dtheta_c < -M_PI) dtheta_c += 2 * M_PI;
        vel_c.linear.x = sqrt(dx_c * dx_c + dy_c * dy_c);
        vel_c.angular.z = ANGULAR_VEL * dtheta_c;

        turtle_a.vel_pub_.publish(vel_a);
        turtle_b.vel_pub_.publish(vel_b);
        turtle_c.vel_pub_.publish(vel_c);

        rate.sleep();
    }

    return 0;
}

该代码中创建了三个 TurtleController 对象，分别对应三只乌龟 A、B、C。每个 TurtleController 对象订阅键盘控制指令，并将其转换为运动指令发布给对应的乌龟。同时，在主循环中计算三只乌龟的位置，然后分别计算出 B、C 相对于 A 的运动指令，使它们始终保持等边三角形的编队。其中，角速度为常数 ANGULAR_VEL，边长为常数 SIDE_LENGTH。