ROS C++ 编写的乌龟等边三角形编队控制

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

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

#define PI 3.14159265358979323846

class TurtleController {
public:
    TurtleController(std::string name, double x, double y) {
        this->name = name;
        this->x = x;
        this->y = y;
        this->theta = 0;
        this->vel_pub = nh.advertise<geometry_msgs::Twist>('/' + name + '/cmd_vel', 10);
        this->pose_sub = nh.subscribe('/' + name + '/pose', 10, &TurtleController::poseCallback, this);
        this->target_x = x + cos(theta) * 1;
        this->target_y = y + sin(theta) * 1;
    }

    void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
        this->x = msg->x;
        this->y = msg->y;
        this->theta = msg->theta;
        this->target_x = x + cos(theta) * 1;
        this->target_y = y + sin(theta) * 1;
    }

    void move(double linear_vel, double angular_vel) {
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = linear_vel;
        vel_msg.angular.z = angular_vel;
        vel_pub.publish(vel_msg);
    }

    void moveToTarget() {
        double distance = sqrt(pow(target_x - x, 2) + pow(target_y - y, 2));
        if (distance > 0.1) {
            double angle = atan2(target_y - y, target_x - x);
            double angular_vel = angle - theta;
            if (angular_vel > PI) angular_vel -= 2 * PI;
            if (angular_vel < -PI) angular_vel += 2 * PI;
            angular_vel = std::max(std::min(angular_vel, 2.0), -2.0);
            move(1.0, angular_vel);
        } else {
            move(0.0, 0.0);
        }
    }

    std::string name;
    double x, y, theta;
    double target_x, target_y;

private:
    ros::NodeHandle nh;
    ros::Publisher vel_pub;
    ros::Subscriber pose_sub;
};

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

    TurtleController turtle_a('turtle1', 5.0, 5.0);
    TurtleController turtle_b('turtle2', 5.0, 6.0);
    TurtleController turtle_c('turtle3', 5.0, 7.0);

    ros::Rate loop_rate(10);

    while (ros::ok()) {
        turtle_a.moveToTarget();
        turtle_b.target_x = turtle_a.target_x + cos(turtle_a.theta + 2 * PI / 3) * 1;
        turtle_b.target_y = turtle_a.target_y + sin(turtle_a.theta + 2 * PI / 3) * 1;
        turtle_b.moveToTarget();
        turtle_c.target_x = turtle_a.target_x + cos(turtle_a.theta - 2 * PI / 3) * 1;
        turtle_c.target_y = turtle_a.target_y + sin(turtle_a.theta - 2 * PI / 3) * 1;
        turtle_c.moveToTarget();
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

在该代码中，我们定义了一个 TurtleController 类来控制乌龟的运动。在构造函数中，我们初始化了乌龟的位置和朝向，并订阅了乌龟的位置信息。在 move 函数中，我们通过发布 geometry_msgs::Twist 消息来控制乌龟的运动。在 moveToTarget 函数中，我们计算了乌龟当前位置和目标位置之间的距离和角度差，并通过调整线速度和角速度来使乌龟朝向目标位置运动。在 main 函数中，我们创建了三只乌龟，并在循环中不断更新它们的目标位置，使它们始终保持等边三角形的编队。