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

以下是一个可能的实现，其中使用了 turtlebot3 模拟器和键盘控制节点。

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

// 定义全局变量
const double PI = 3.14159265358979323846;
const double SIDE_LENGTH = 1.0;
const double ANGLE = 2 * PI / 3;

// 定义回调函数
void poseCallbackA(const turtlesim::Pose::ConstPtr& poseA, ros::Publisher& pubB, ros::Publisher& pubC)
{
    // 计算B和C的位置
    double xB = poseA->x + SIDE_LENGTH * cos(poseA->theta + ANGLE);
    double yB = poseA->y + SIDE_LENGTH * sin(poseA->theta + ANGLE);
    double xC = poseA->x + SIDE_LENGTH * cos(poseA->theta - ANGLE);
    double yC = poseA->y + SIDE_LENGTH * sin(poseA->theta - ANGLE);

    // 发布控制指令
    geometry_msgs::Twist twistB, twistC;
    twistB.linear.x = twistC.linear.x = 0.0;
    twistB.angular.z = twistC.angular.z = 0.0;
    twistB.linear.x = twistC.linear.x = 0.5;
    twistB.angular.z = twistC.angular.z = 0.0;
    pubB.publish(twistB);
    pubC.publish(twistC);
}

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

    // 创建发布器和订阅器
    ros::Publisher pubA = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
    ros::Publisher pubB = nh.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 1000);
    ros::Publisher pubC = nh.advertise<geometry_msgs::Twist>("turtle3/cmd_vel", 1000);
    ros::Subscriber subA = nh.subscribe<turtlesim::Pose>("turtle1/pose", 1000, boost::bind(poseCallbackA, _1, pubB, pubC));

    // 循环发布控制指令
    ros::Rate rate(10);
    while (ros::ok())
    {
        // 获取键盘输入
        char input;
        std::cout << "Enter a key: ";
        std::cin >> input;

        // 根据输入发布控制指令
        geometry_msgs::Twist twistA;
        twistA.linear.x = twistA.linear.y = twistA.angular.z = 0.0;
        switch (input)
        {
            case 'w':
                twistA.linear.x = 0.5;
                break;
            case 'a':
                twistA.angular.z = 0.5;
                break;
            case 's':
                twistA.linear.x = -0.5;
                break;
            case 'd':
                twistA.angular.z = -0.5;
                break;
        }
        pubA.publish(twistA);

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

    return 0;
}

该代码中，我们定义了一个回调函数poseCallbackA，用于计算 B 和 C 的位置，并发布控制指令，使得 B 和 C 始终跟随 A 形成等边三角形。在主函数中，我们创建了三个发布器和一个订阅器，分别用于控制 A、B、C 的运动，并循环等待键盘输入。当接收到键盘输入后，我们根据输入发布控制指令，控制 A 的运动。同时，回调函数会被触发，计算出 B 和 C 的位置，并发布控制指令，使得 B 和 C 始终跟随 A 形成等边三角形。

需要注意的是，该代码假设 turtlebot3 的初始位置为 (0, 0)，且朝向为 x 轴正方向。如果 turtlebot3 的初始位置和朝向不同，需要对代码进行相应的修改。