ROS C++教程：实现三只乌龟的等边三角形编队控制

本教程将教你如何使用ROS C++编写代码，实现控制三只乌龟（命名为A、B、C）始终保持边长为1的等边三角形编队运动，并保持朝向一致。

代码实现

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

// 定义全局变量
ros::Publisher turtleAPub;
ros::Publisher turtleBPub;
ros::Publisher turtleCPub;
geometry_msgs::Twist turtleAVel;
geometry_msgs::Twist turtleBVel;
geometry_msgs::Twist turtleCVel;

// 控制A乌龟运动的回调函数
void turtleACallback(const geometry_msgs::Twist& msg) {
    turtleAVel = msg;

    // 计算B、C乌龟的位置
    double theta = M_PI / 3; // 120度
    double x = cos(theta);
    double y = sin(theta);
    turtleBVel.linear.x = -x * turtleAVel.linear.x + y * turtleAVel.linear.y;
    turtleBVel.linear.y = -y * turtleAVel.linear.x - x * turtleAVel.linear.y;
    turtleBVel.angular.z = turtleAVel.angular.z;

    turtleCVel.linear.x = -x * turtleAVel.linear.x - y * turtleAVel.linear.y;
    turtleCVel.linear.y = y * turtleAVel.linear.x - x * turtleAVel.linear.y;
    turtleCVel.angular.z = turtleAVel.angular.z;

    // 发布速度指令
    turtleAPub.publish(turtleAVel);
    turtleBPub.publish(turtleBVel);
    turtleCPub.publish(turtleCVel);
}

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

    // 创建三只乌龟的速度控制器
    turtleAPub = nh.advertise<geometry_msgs::Twist>('/turtleA/cmd_vel', 10);
    turtleBPub = nh.advertise<geometry_msgs::Twist>('/turtleB/cmd_vel', 10);
    turtleCPub = nh.advertise<geometry_msgs::Twist>('/turtleC/cmd_vel', 10);

    // 订阅A乌龟的速度指令
    ros::Subscriber turtleASub = nh.subscribe('/turtleA/cmd_vel', 10, turtleACallback);

    // 循环执行
    ros::spin();

    return 0;
}

代码解释

包含头文件:
- ros/ros.h: ROS核心头文件。
- geometry_msgs/Twist.h: 用于发布速度指令。
- cmath: 数学函数库，用于计算三角函数。
定义全局变量:
- turtleAPub, turtleBPub, turtleCPub: 三个ROS Publisher，用于发布三只乌龟的速度指令。
- turtleAVel, turtleBVel, turtleCVel: 三个geometry_msgs::Twist类型的变量，存储三只乌龟的速度信息。
回调函数 turtleACallback:
- 接收A乌龟的速度指令 msg。
- 计算B、C乌龟的位置，使其与A保持等边三角形，并保持朝向一致。
- 通过turtleAPub，turtleBPub，turtleCPub发布三只乌龟的速度指令。
主函数 main:
- 初始化ROS节点，命名为'formation_control'。
- 创建三个Publisher，分别用于发布A、B、C三只乌龟的速度指令。
- 创建一个Subscriber，订阅A乌龟的速度指令，并绑定回调函数turtleACallback。
- 进入ros::spin()循环，等待回调函数被触发。

注意事项

该代码假设你已经启动了ROS Master，并在环境中正确加载了turtlesim包。
你需要使用键盘控制节点（例如teleop_twist_keyboard）来发布A乌龟的速度指令。
可以根据需要修改代码，实现不同形状的编队控制。