ROS C++ 编写的乌龟等边三角形编队控制

以下是一个可能的实现：

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

// 定义全局变量
ros::Publisher turtle_a_pub, turtle_b_pub, turtle_c_pub;
geometry_msgs::Twist turtle_a_cmd, turtle_b_cmd, turtle_c_cmd;
double side_length = 1.0;

// 键盘控制回调函数
void keyboardCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
  // 控制乌龟A移动
  turtle_a_cmd = *msg;

  // 计算乌龟B和乌龟C的位置
  double angle = atan2(turtle_a_cmd.linear.y, turtle_a_cmd.linear.x);
  double x_offset = side_length * cos(angle + M_PI/3);
  double y_offset = side_length * sin(angle + M_PI/3);

  // 发布控制指令
  turtle_b_cmd.linear.x = turtle_a_cmd.linear.x + x_offset;
  turtle_b_cmd.linear.y = turtle_a_cmd.linear.y + y_offset;
  turtle_b_cmd.angular.z = turtle_a_cmd.angular.z;

  turtle_c_cmd.linear.x = turtle_a_cmd.linear.x - x_offset;
  turtle_c_cmd.linear.y = turtle_a_cmd.linear.y - y_offset;
  turtle_c_cmd.angular.z = turtle_a_cmd.angular.z;

  turtle_a_pub.publish(turtle_a_cmd);
  turtle_b_pub.publish(turtle_b_cmd);
  turtle_c_pub.publish(turtle_c_cmd);
}

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

  // 创建三只乌龟的发布者
  turtle_a_pub = nh.advertise<geometry_msgs::Twist>('turtle1/cmd_vel', 1);
  turtle_b_pub = nh.advertise<geometry_msgs::Twist>('turtle2/cmd_vel', 1);
  turtle_c_pub = nh.advertise<geometry_msgs::Twist>('turtle3/cmd_vel', 1);

  // 创建键盘控制的订阅者
  ros::Subscriber keyboard_sub = nh.subscribe<geometry_msgs::Twist>('cmd_vel', 1, keyboardCallback);

  // 设置乌龟A的初始速度
  turtle_a_cmd.linear.x = 0.5;

  // 循环执行
  ros::Rate rate(10);
  while (ros::ok())
  {
    // 发布乌龟A的控制指令
    turtle_a_pub.publish(turtle_a_cmd);

    // 等待回调函数触发
    ros::spinOnce();

    // 控制频率
    rate.sleep();
  }

  return 0;
}

在这个程序中，我们定义了三个全局变量 turtle_a_pub、turtle_b_pub、turtle_c_pub，分别表示三只乌龟的发布者。在键盘控制的回调函数中，我们首先将键盘控制的指令赋给乌龟A的控制指令 turtle_a_cmd，然后根据当前角度计算出乌龟B和乌龟C的位置，并将控制指令分别发布到三只乌龟的话题上。

注意，在这个程序中，我们假设乌龟A的初速度为 (0.5, 0, 0)，即沿着x轴正方向前进。如果你想改变这个初速度，可以在程序中修改 turtle_a_cmd.linear.x 的值。