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

本文介绍了使用 ROS CPP 编写代码，实现三只乌龟在 ROS 模拟环境中以等边三角形编队移动的功能。代码通过键盘控制乌龟 A 的运动，并根据三角形几何关系实时计算乌龟 B 和乌龟 C 的位置和方向，实现编队效果。

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

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

// 乌龟A的坐标和方向
float turtleA_x = 5.54;
float turtleA_y = 5.54;
float turtleA_theta = 0;

// 乌龟B的坐标和方向
float turtleB_x = 5.54;
float turtleB_y = 5.54;
float turtleB_theta = 0;

// 乌龟C的坐标和方向
float turtleC_x = 5.54;
float turtleC_y = 5.54;
float turtleC_theta = 0;

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

// 计算两点之间的角度
float angle(float x1, float y1, float x2, float y2) {
    return std::atan2(y2 - y1, x2 - x1);
}

// 控制乌龟A移动
void moveTurtleA(ros::Publisher& pub, float linear, float angular) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear;
    msg.angular.z = angular;
    pub.publish(msg);
}

// 控制乌龟B移动
void moveTurtleB(ros::Publisher& pub, float linear, float angular) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear;
    msg.angular.z = angular;
    pub.publish(msg);
}

// 控制乌龟C移动
void moveTurtleC(ros::Publisher& pub, float linear, float angular) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear;
    msg.angular.z = angular;
    pub.publish(msg);
}

// 接收乌龟A的位置信息
void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& msg) {
    turtleA_x = msg->x;
    turtleA_y = msg->y;
    turtleA_theta = msg->theta;
}

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

    // 创建三只乌龟的发布器和订阅器
    ros::Publisher turtleAPub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
    ros::Subscriber turtleASub = nh.subscribe("/turtle1/pose", 1000, turtleAPoseCallback);
    ros::Publisher turtleBPub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);
    ros::Publisher turtleCPub = nh.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 1000);

    // 控制频率为10Hz
    ros::Rate rate(10);

    while (ros::ok()) {
        // 计算乌龟B和乌龟C的位置和方向
        float d = 1;
        float theta = turtleA_theta;
        float turtleB_x_new = turtleA_x + d * std::cos(theta + 2 * M_PI / 3);
        float turtleB_y_new = turtleA_y + d * std::sin(theta + 2 * M_PI / 3);
        float turtleC_x_new = turtleA_x + d * std::cos(theta - 2 * M_PI / 3);
        float turtleC_y_new = turtleA_y + d * std::sin(theta - 2 * M_PI / 3);
        float turtleB_theta_new = angle(turtleA_x, turtleA_y, turtleB_x_new, turtleB_y_new);
        float turtleC_theta_new = angle(turtleA_x, turtleA_y, turtleC_x_new, turtleC_y_new);

        // 控制乌龟B和乌龟C移动
        float linear = 1;
        float angular = turtleB_theta_new - turtleB_theta;
        moveTurtleB(turtleBPub, linear, angular);
        angular = turtleC_theta_new - turtleC_theta;
        moveTurtleC(turtleCPub, linear, angular);

        // 控制乌龟A移动
        ros::spinOnce();
        linear = 1;
        angular = 0;
        moveTurtleA(turtleAPub, linear, angular);

        rate.sleep();
    }

    return 0;
}

代码说明：

首先定义了三只乌龟的坐标和方向变量：turtleA_x、turtleA_y、turtleA_theta、turtleB_x、turtleB_y、turtleB_theta、turtleC_x、turtleC_y、turtleC_theta。
定义了两个函数：distance 和 angle，用于计算两点之间的距离和角度。
定义了三个函数：moveTurtleA、moveTurtleB 和 moveTurtleC，用于控制三只乌龟的移动。
定义了一个回调函数 turtleAPoseCallback，用于接收乌龟A的位置信息，并更新 turtleA_x、turtleA_y、turtleA_theta。
在 main 函数中，创建了三只乌龟的发布器和订阅器，以及控制频率为 10Hz 的循环。
在循环中，首先计算出乌龟B和乌龟C的位置和方向，然后分别控制乌龟B和乌龟C移动，最后接收乌龟A的位置信息并控制乌龟A移动。

**注意：**该代码中只实现了键盘控制乌龟A移动，没有实现通过键盘控制乌龟A的方向。如果需要实现该功能，可以通过订阅 /turtle1/cmd_vel 主题，获取键盘控制的速度和角速度，然后根据速度和角速度计算出乌龟A的方向。