ROS C++ 代码实现乌龟等边三角形编队

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

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

// Constants
const double DISTANCE = 1.0; // Distance between turtles
const double ANGLE = M_PI / 3; // Angle between turtles
const double LINEAR_SPEED = 1.0; // Linear speed of turtle A
const double ANGULAR_SPEED = 1.0; // Angular speed of turtle A

// Global variables
ros::Publisher turtleAPub; // Publisher for turtle A
ros::Subscriber turtleASub; // Subscriber for turtle A
ros::Publisher turtleBPub; // Publisher for turtle B
ros::Publisher turtleCPub; // Publisher for turtle C
bool isMoving; // Flag to indicate whether turtle A is moving or not

// Callback function for turtle A pose
void turtleACallback(const turtlesim::Pose::ConstPtr& msg) {
  if (isMoving) {
    // Calculate the desired pose for turtle B
    double xB = msg->x + DISTANCE * cos(msg->theta + ANGLE);
    double yB = msg->y + DISTANCE * sin(msg->theta + ANGLE);
    double thetaB = msg->theta + ANGLE;

    // Calculate the desired pose for turtle C
    double xC = msg->x + DISTANCE * cos(msg->theta - ANGLE);
    double yC = msg->y + DISTANCE * sin(msg->theta - ANGLE);
    double thetaC = msg->theta - ANGLE;

    // Publish the desired poses for turtles B and C
    geometry_msgs::Twist turtleBMsg;
    turtleBMsg.linear.x = 0;
    turtleBMsg.angular.z = 0;
    turtleBMsg.linear.x = LINEAR_SPEED;
    turtleBMsg.angular.z = ANGULAR_SPEED;
    turtleBPub.publish(turtleBMsg);

    geometry_msgs::Twist turtleCMsg;
    turtleCMsg.linear.x = 0;
    turtleCMsg.angular.z = 0;
    turtleCMsg.linear.x = LINEAR_SPEED;
    turtleCMsg.angular.z = ANGULAR_SPEED;
    turtleCPub.publish(turtleCMsg);
  }
}

// Main function
int main(int argc, char** argv) {
  // Initialize ROS node
  ros::init(argc, argv, "turtle_triangle");
  ros::NodeHandle nh;

  // Create publishers and subscribers
  turtleAPub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
  turtleASub = nh.subscribe("/turtle1/pose", 1000, turtleACallback);
  turtleBPub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);
  turtleCPub = nh.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 1000);

  // Set the flag to indicate turtle A is not moving initially
  isMoving = false;

  // Wait for the first pose message from turtle A
  while (ros::ok() && !isMoving) {
    ros::spinOnce();
  }

  // Start moving turtle A
  geometry_msgs::Twist turtleAMsg;
  turtleAMsg.linear.x = LINEAR_SPEED;
  turtleAMsg.angular.z = ANGULAR_SPEED;
  turtleAPub.publish(turtleAMsg);
  isMoving = true;

  // Spin and wait for callbacks
  ros::spin();

  return 0;
}

该代码中，我们首先定义了三个常量：DISTANCE表示两只乌龟之间的距离，ANGLE表示两只乌龟之间的夹角，LINEAR_SPEED和ANGULAR_SPEED分别表示乌龟 A 的线速度和角速度。

然后，我们定义了四个全局变量：turtleAPub表示乌龟 A 的发布者，turtleASub表示乌龟 A 的订阅者，turtleBPub和turtleCPub分别表示乌龟 B 和乌龟 C 的发布者。isMoving是一个标志，用于指示乌龟 A 是否正在移动。

在turtleACallback回调函数中，我们首先判断乌龟 A 是否正在移动，如果是，则计算乌龟 B 和乌龟 C 的期望位置，并将期望位置发布到相应的话题上。

在main函数中，我们首先初始化 ROS 节点，并创建发布者和订阅者。然后，我们将isMoving标志设置为false，并等待来自乌龟 A 的第一个姿态消息。一旦收到消息，我们就开始让乌龟 A 移动，并将isMoving标志设置为true。最后，我们进入 ROS 的主循环，等待回调函数的调用。

需要注意的是，该代码中假设乌龟 B 和乌龟 C 的初始位置与乌龟 A 相同，因此它们会随着乌龟 A 的移动而移动。如果需要指定它们的初始位置，可以在代码中进行相应的修改。