ROS C++教程：实现键盘控制三只乌龟保持等边三角形队形

本教程将引导你使用ROS C++编写代码，实现以下功能：

创建三只乌龟A、B、C，并设置它们的初始位置，使其形成等边三角形。
通过键盘控制乌龟A的运动（前进、后退、转向）。
确保无论乌龟A如何运动，乌龟B和C都能始终与其保持等边三角形的队形，且朝向一致。

代码实现

以下是实现上述功能的ROS C++代码：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

// 全局变量
ros::Publisher pubA, pubB, pubC;
float xA = 5.0, yA = 5.0, thetaA = 0.0; // 乌龟A的初始位置和朝向
float xB = 5.866, yB = 5.0, thetaB = 2.094; // 乌龟B的初始位置和朝向
float xC = 5.433, yC = 5.866, thetaC = 4.189; // 乌龟C的初始位置和朝向

// 回调函数，控制乌龟A运动
void moveTurtleA(const geometry_msgs::Twist::ConstPtr& msg)
{
    // 发布速度指令
    pubA.publish(msg);

    // 计算乌龟B和乌龟C的位置和朝向
    float d = 1.0; // 三角形边长
    float dx = d * cos(thetaA);
    float dy = d * sin(thetaA);
    float thetaB = thetaA + 2.094; // 乌龟B的朝向
    float thetaC = thetaA - 2.094; // 乌龟C的朝向
    float xB = xA + dx * cos(thetaB) - dy * sin(thetaB); // 乌龟B的x坐标
    float yB = yA + dx * sin(thetaB) + dy * cos(thetaB); // 乌龟B的y坐标
    float xC = xA + dx * cos(thetaC) - dy * sin(thetaC); // 乌龟C的x坐标
    float yC = yA + dx * sin(thetaC) + dy * cos(thetaC); // 乌龟C的y坐标

    // 发布乌龟B和乌龟C的速度指令
    geometry_msgs::Twist msgB, msgC;
    msgB.linear.x = msgC.linear.x = msg->linear.x;
    msgB.angular.z = msgC.angular.z = msg->angular.z;
    pubB.publish(msgB);
    pubC.publish(msgC);
}

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

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

    // 创建乌龟A
    ros::ServiceClient spawnClient = nh.serviceClient<turtlesim::Spawn>('spawn');
    turtlesim::Spawn::Request req;
    turtlesim::Spawn::Response resp;
    req.x = xA;
    req.y = yA;
    req.theta = thetaA;
    req.name = 'turtleA';
    spawnClient.call(req, resp);

    // 创建乌龟B
    req.x = xB;
    req.y = yB;
    req.theta = thetaB;
    req.name = 'turtleB';
    spawnClient.call(req, resp);

    // 创建乌龟C
    req.x = xC;
    req.y = yC;
    req.theta = thetaC;
    req.name = 'turtleC';
    spawnClient.call(req, resp);

    // 创建乌龟A的速度控制器
    pubA = nh.advertise<geometry_msgs::Twist>('/turtleA/cmd_vel', 10);

    // 创建乌龟B的速度控制器
    pubB = nh.advertise<geometry_msgs::Twist>('/turtleB/cmd_vel', 10);

    // 创建乌龟C的速度控制器
    pubC = nh.advertise<geometry_msgs::Twist>('/turtleC/cmd_vel', 10);

    // 创建键盘控制乌龟A的订阅者
    ros::Subscriber sub = nh.subscribe('/turtleA/cmd_vel', 10, moveTurtleA);

    // 进入循环
    ros::spin();

    return 0;
}

代码解释：

包含头文件: 包含必要的 ROS 和 turtlesim 库的头文件。
全局变量: 定义全局变量以存储乌龟A、B、C的位置和朝向信息，以及用于控制它们运动的 ROS 发布器。
回调函数 moveTurtleA:
- 接收键盘控制指令 (geometry_msgs::Twist) 并将其发送给乌龟A。
- 根据乌龟A的当前位置和朝向，计算乌龟B 和 C 应该处于的位置，以保持等边三角形的队形。
- 将计算出的速度指令发送给乌龟 B 和 C，使它们与乌龟 A 同步运动。
主函数 main:
- 初始化 ROS 节点。
- 创建三个乌龟 (A、B、C)，并设置它们的初始位置，使其形成等边三角形。
- 创建速度发布器，用于控制每个乌龟的运动。
- 创建订阅器，监听键盘控制指令，并将指令传递给 moveTurtleA 回调函数。
- 进入 ros::spin() 循环，持续处理 ROS 事件。

运行程序

保存代码为 turtle_triangle.cpp 文件。
使用 catkin 编译代码。
在终端中启动 roscore: roscore。
在新的终端中运行节点: rosrun your_package_name turtle_triangle。
在另一个终端中使用键盘控制节点发布速度指令，例如：rostopic pub /turtleA/cmd_vel geometry_msgs/Twist '{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}'。

现在你可以观察到三只乌龟保持等边三角形队形移动！