使用 ROS C++ 实现三只乌龟等边三角形编队

本代码使用 ROS C++ 语言，实现三只乌龟在模拟环境中保持等边三角形编队。用户可以通过键盘控制第一只乌龟的运动，其余两只乌龟会自动调整位置，始终保持等边三角形的形状。

代码

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

// 定义全局变量，表示三只乌龟的位置
turtlesim::Pose turtleA_pose;
turtlesim::Pose turtleB_pose;
turtlesim::Pose turtleC_pose;

// 定义回调函数，获取乌龟A的位置信息
void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    turtleA_pose = *msg;
}

// 定义回调函数，获取乌龟B的位置信息
void turtleBPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    turtleB_pose = *msg;
}

// 定义回调函数，获取乌龟C的位置信息
void turtleCPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    turtleC_pose = *msg;
}

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

    // 创建节点句柄
    ros::NodeHandle nh;

    // 创建三个发布者，用于控制三只乌龟的运动
    ros::Publisher turtleAPub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
    ros::Publisher turtleBPub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);
    ros::Publisher turtleCPub = nh.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 1000);

    // 创建三个订阅者，用于获取三只乌龟的位置信息
    ros::Subscriber turtleASub = nh.subscribe("/turtle1/pose", 1000, turtleAPoseCallback);
    ros::Subscriber turtleBSub = nh.subscribe("/turtle2/pose", 1000, turtleBPoseCallback);
    ros::Subscriber turtleCSub = nh.subscribe("/turtle3/pose", 1000, turtleCPoseCallback);

    // 设置循环的频率
    ros::Rate loop_rate(10);

    while (ros::ok())
    {
        // 获取键盘输入的控制指令
        char c = getchar();

        // 根据不同的指令控制乌龟A的运动
        geometry_msgs::Twist turtleA_cmd;
        switch (c)
        {
        case 'w':
            turtleA_cmd.linear.x = 1.0;
            break;
        case 's':
            turtleA_cmd.linear.x = -1.0;
            break;
        case 'a':
            turtleA_cmd.angular.z = 1.0;
            break;
        case 'd':
            turtleA_cmd.angular.z = -1.0;
            break;
        default:
            break;
        }
        turtleAPub.publish(turtleA_cmd);

        // 计算乌龟B和乌龟C应该移动的距离和角度
        double distance = 1.0;
        double angle = M_PI / 3.0;
        double delta_x = distance * cos(angle);
        double delta_y = distance * sin(angle);

        // 计算乌龟B和乌龟C应该移动到的目标位置
        double target_x = turtleA_pose.x + delta_x;
        double target_y = turtleA_pose.y + delta_y;

        // 计算乌龟B和乌龟C应该朝向的角度
        double target_theta = turtleA_pose.theta + angle;

        // 计算乌龟B和乌龟C当前位置与目标位置的距离和角度
        double dx = turtleB_pose.x - target_x;
        double dy = turtleB_pose.y - target_y;
        double distance_B = sqrt(dx * dx + dy * dy);
        double angle_B = atan2(dy, dx) - turtleB_pose.theta;

        dx = turtleC_pose.x - target_x;
        dy = turtleC_pose.y - target_y;
        double distance_C = sqrt(dx * dx + dy * dy);
        double angle_C = atan2(dy, dx) - turtleC_pose.theta;

        // 根据距离和角度的差距控制乌龟B和乌龟C的运动
        geometry_msgs::Twist turtleB_cmd;
        if (distance_B > 0.1)
        {
            turtleB_cmd.linear.x = 0.5 * distance_B;
            turtleB_cmd.angular.z = 4.0 * angle_B;
        }
        turtleBPub.publish(turtleB_cmd);

        geometry_msgs::Twist turtleC_cmd;
        if (distance_C > 0.1)
        {
            turtleC_cmd.linear.x = 0.5 * distance_C;
            turtleC_cmd.angular.z = 4.0 * angle_C;
        }
        turtleCPub.publish(turtleC_cmd);

        // 循环等待
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

说明

1. 代码中定义了三个全局变量，分别表示三只乌龟的位置信息 turtleA_pose、turtleB_pose 和 turtleC_pose。
2. 代码定义了三个回调函数，分别用于获取三只乌龟的位置信息。
3. 代码创建了三个发布者，用于控制三只乌龟的运动。
4. 代码创建了三个订阅者，用于获取三只乌龟的位置信息。
5. 在循环中，代码首先获取键盘输入的控制指令，然后根据指令控制乌龟A的运动。
6. 接着，代码计算乌龟B和乌龟C应该移动的距离和角度，以及它们应该移动到的目标位置。
7. 然后，代码根据乌龟B和乌龟C当前位置与目标位置的距离和角度差距，控制它们的运动。
8. 最后，代码循环等待，直到程序退出。

运行

1. 确保已经安装了 ROS 和 turtlesim 包。
2. 在终端中运行以下命令启动 turtlesim 节点：

roslaunch turtlesim turtlesim_node.launch

3. 在另一个终端中运行以下命令编译并运行代码：

rosrun your_package your_node_name

其中 your_package 和 your_node_name 分别为你的包名和节点名。

4. 按键盘上的方向键 (w、a、s、d) 控制乌龟A的运动，观察乌龟B和乌龟C如何自动调整位置，始终保持等边三角形的形状。

注意

• 代码中使用了 getchar() 函数获取键盘输入，它只接受单个字符的输入，并且不会显示在终端窗口中。
• 代码中的运动速度和角度控制参数可以根据实际情况进行调整。
• 这只是一个简单的实现示例，可以根据自己的需求进行扩展和修改。