ROS CPP Turtlebot 编队控制：实现等边三角形编队

以下是实现上述功能的代码，其中使用了 ROS 中的 turtle_sim 包来模拟乌龟的运动：

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

// 定义全局变量
ros::Publisher pubA, pubB, pubC;
turtlesim::Pose poseA, poseB, poseC;
double distance = 1.0;
double angle = 0.0;

// 回调函数，处理乌龟 A 的位置信息
void poseCallbackA(const turtlesim::Pose::ConstPtr& msg)
{
  poseA = *msg;
}

// 回调函数，处理乌龟 B 的位置信息
void poseCallbackB(const turtlesim::Pose::ConstPtr& msg)
{
  poseB = *msg;
}

// 回调函数，处理乌龟 C 的位置信息
void poseCallbackC(const turtlesim::Pose::ConstPtr& msg)
{
  poseC = *msg;
}

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

// 控制乌龟 A 运动
void moveTurtleA()
{
  // 创建 Twist 消息
  geometry_msgs::Twist twist;

  // 计算目标位置
  double targetX = poseA.x + distance * std::cos(angle);
  double targetY = poseA.y + distance * std::sin(angle);

  // 计算目标角度
  double targetAngle = std::atan2(poseB.y-poseA.y, poseB.x-poseA.x);

  // 计算角速度
  double angularVel = 4.0 * (targetAngle - poseA.theta);

  // 设置线速度和角速度
  twist.linear.x = 2.0 * getDistance(poseA.x, poseA.y, targetX, targetY);
  twist.angular.z = angularVel;

  // 发布 Twist 消息
  pubA.publish(twist);
}

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

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

  // 创建三个 Publisher，分别用于控制乌龟 A、B、C
  pubA = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
  pubB = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);
  pubC = nh.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 1000);

  // 创建三个 Subscriber，分别用于接收乌龟 A、B、C 的位置信息
  ros::Subscriber subA = nh.subscribe("/turtle1/pose", 1000, poseCallbackA);
  ros::Subscriber subB = nh.subscribe("/turtle2/pose", 1000, poseCallbackB);
  ros::Subscriber subC = nh.subscribe("/turtle3/pose", 1000, poseCallbackC);

  // 等待乌龟 A、B、C 的位置信息
  while (ros::ok() && (poseA.x == 0 || poseB.x == 0 || poseC.x == 0))
  {
    ros::spinOnce();
  }

  // 使乌龟 B 和 C 围绕乌龟 A 旋转，保持等边三角形的形状
  while (ros::ok())
  {
    // 计算目标角度
    double targetAngleB = poseA.theta + 2.0/3.0*M_PI;
    double targetAngleC = poseA.theta - 2.0/3.0*M_PI;

    // 创建 Twist 消息
    geometry_msgs::Twist twistB, twistC;

    // 设置线速度和角速度
    twistB.linear.x = 2.0 * distance;
    twistB.angular.z = 4.0 * (targetAngleB - poseB.theta);

    twistC.linear.x = 2.0 * distance;
    twistC.angular.z = 4.0 * (targetAngleC - poseC.theta);

    // 发布 Twist 消息
    pubB.publish(twistB);
    pubC.publish(twistC);

    // 控制乌龟 A 运动
    moveTurtleA();

    // 等待一段时间
    ros::Duration(0.1).sleep();

    // 检查是否收到退出信号
    ros::spinOnce();
  }

  return 0;
}

在上述代码中，我们使用了三个 Subscriber 来接收乌龟 A、B、C 的位置信息，然后根据当前位置和目标位置计算出线速度和角速度，最后发布 Twist 消息来控制乌龟的运动。

在主循环中，我们首先等待乌龟 A、B、C 的位置信息，然后让乌龟 B 和 C 围绕乌龟 A 旋转，保持等边三角形的形状。接着，我们不断调用 moveTurtleA 函数来控制乌龟 A 的运动，使其朝着目标位置移动。最后，我们等待一段时间，然后检查是否收到退出信号，以便程序能够正常退出。