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

以下是使用 ROS C++ 编写的代码，实现产生两只乌龟 A、B、C，并通过键盘控制 A 运动时，A、B、C 始终保持等边三角形的编队，边长为 1，朝向一致。

代码解析：

1. 首先在 ROS 中创建一个节点，定义三只乌龟的名称和初始位置。

2. 通过订阅 /turtle1/pose 话题，获取乌龟 A 的位置信息，并计算出乌龟 B 和乌龟 C 应该移动的目标位置。

3. 通过发布 /turtle2/cmd_vel 和 /turtle3/cmd_vel 话题，控制乌龟 B 和乌龟 C 移动到目标位置。

4. 通过键盘控制乌龟 A 的运动，当乌龟 A 移动时，乌龟 B 和乌龟 C 也会同时移动，保持等边三角形的编队。

完整代码如下：

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

#define PI 3.14159265358979323846

// 定义三只乌龟的名称和初始位置
std::string turtle1_name = 'turtle1';
double turtle1_x = 5.54;
double turtle1_y = 5.54;
double turtle1_theta = 0.0;

std::string turtle2_name = 'turtle2';
double turtle2_x = 4.54;
double turtle2_y = 5.54;
double turtle2_theta = 0.0;

std::string turtle3_name = 'turtle3';
double turtle3_x = 6.54;
double turtle3_y = 5.54;
double turtle3_theta = 0.0;

// 订阅/turtle1/pose话题，获取乌龟A的位置信息
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
  turtle1_x = msg->x;
  turtle1_y = msg->y;
  turtle1_theta = msg->theta;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, 'turtle_control');
  ros::NodeHandle nh;

  // 创建发布/turtle2/cmd_vel和/turtle3/cmd_vel话题的发布器
  ros::Publisher turtle2_vel_pub = nh.advertise<geometry_msgs::Twist>(turtle2_name+'/cmd_vel', 1000);
  ros::Publisher turtle3_vel_pub = nh.advertise<geometry_msgs::Twist>(turtle3_name+'/cmd_vel', 1000);

  // 创建订阅/turtle1/pose话题的订阅器
  ros::Subscriber pose_sub = nh.subscribe<turtlesim::Pose>(turtle1_name+'/pose', 1000, poseCallback);

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

  while (ros::ok())
  {
    // 计算乌龟B和乌龟C应该移动的目标位置
    double target_x = turtle1_x + cos(turtle1_theta + PI/3);
    double target_y = turtle1_y + sin(turtle1_theta + PI/3);

    // 计算乌龟B和乌龟C到目标位置的距离和角度
    double distance2 = sqrt(pow(target_x - turtle2_x, 2) + pow(target_y - turtle2_y, 2));
    double angle2 = atan2(target_y - turtle2_y, target_x - turtle2_x);

    double distance3 = sqrt(pow(target_x - turtle3_x, 2) + pow(target_y - turtle3_y, 2));
    double angle3 = atan2(target_y - turtle3_y, target_x - turtle3_x);

    // 创建geometry_msgs::Twist消息，设置线速度和角速度
    geometry_msgs::Twist vel_msg2;
    vel_msg2.linear.x = distance2;
    vel_msg2.angular.z = angle2;

    geometry_msgs::Twist vel_msg3;
    vel_msg3.linear.x = distance3;
    vel_msg3.angular.z = angle3;

    // 发布/turtle2/cmd_vel和/turtle3/cmd_vel话题，控制乌龟B和乌龟C移动到目标位置
    turtle2_vel_pub.publish(vel_msg2);
    turtle3_vel_pub.publish(vel_msg3);

    // 处理ROS的回调函数
    ros::spinOnce();

    // 按键盘控制乌龟A的运动
    char c;
    std::cout << 'Enter a key: ';
    std::cin >> c;
    double linear = 1.0;
    double angular = 0.0;
    switch(c)
    {
      case 'w':
        linear = 1.0;
        angular = 0.0;
        break;
      case 'a':
        linear = 0.0;
        angular = 1.0;
        break;
      case 's':
        linear = -1.0;
        angular = 0.0;
        break;
      case 'd':
        linear = 0.0;
        angular = -1.0;
        break;
    }

    // 创建geometry_msgs::Twist消息，设置线速度和角速度
    geometry_msgs::Twist vel_msg1;
    vel_msg1.linear.x = linear;
    vel_msg1.angular.z = angular;

    // 创建发布/turtle1/cmd_vel话题的发布器，控制乌龟A的移动
    ros::Publisher turtle1_vel_pub = nh.advertise<geometry_msgs::Twist>(turtle1_name+'/cmd_vel', 1000);
    turtle1_vel_pub.publish(vel_msg1);

    // 使程序按照设定的循环频率延时
    loop_rate.sleep();
  }

  return 0;
}