ROS C++教程：实现三只乌龟保持等边三角形编队运动

本教程将引导你使用 ROS C++ 编写代码，实现控制三只乌龟 (A、B、C) 始终保持等边三角形编队运动。其中，乌龟A 将通过键盘控制，而乌龟 B 和 C 将跟随 A 运动，并始终保持与 A 形成边长为 1 的等边三角形，且朝向与 A 一致。

代码实现

以下是完整的 ROS C++ 代码：

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

// 定义常量
const double PI = 3.14159265359;
const double LINEAR_SPEED = 1.0;
const double ANGULAR_SPEED = 1.0;

// 定义全局变量
ros::Publisher turtle_pub;
turtlesim::Pose turtle_pose_a, turtle_pose_b, turtle_pose_c;

// 回调函数：获取乌龟A的位置
void turtlePoseCallbackA(const turtlesim::Pose::ConstPtr& msg) {
  turtle_pose_a.x = msg->x;
  turtle_pose_a.y = msg->y;
  turtle_pose_a.theta = msg->theta;
}

// 回调函数：获取乌龟B的位置
void turtlePoseCallbackB(const turtlesim::Pose::ConstPtr& msg) {
  turtle_pose_b.x = msg->x;
  turtle_pose_b.y = msg->y;
  turtle_pose_b.theta = msg->theta;
}

// 回调函数：获取乌龟C的位置
void turtlePoseCallbackC(const turtlesim::Pose::ConstPtr& msg) {
  turtle_pose_c.x = msg->x;
  turtle_pose_c.y = msg->y;
  turtle_pose_c.theta = msg->theta;
}

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

// 计算两点之间的角度
double angle(double x1, double y1, double x2, double y2) {
  return atan2(y2 - y1, x2 - x1);
}

// 控制乌龟A运动
void controlTurtleA() {
  // 计算乌龟A与乌龟B、C之间的距离和角度
  double dist_ab = distance(turtle_pose_a.x, turtle_pose_a.y, turtle_pose_b.x, turtle_pose_b.y);
  double dist_ac = distance(turtle_pose_a.x, turtle_pose_a.y, turtle_pose_c.x, turtle_pose_c.y);
  double angle_ab = angle(turtle_pose_a.x, turtle_pose_a.y, turtle_pose_b.x, turtle_pose_b.y);
  double angle_ac = angle(turtle_pose_a.x, turtle_pose_a.y, turtle_pose_c.x, turtle_pose_c.y);

  // 计算乌龟A应该朝向的角度
  double target_angle = (angle_ab + angle_ac) / 2;

  // 计算乌龟A与目标角度之间的差值
  double delta_angle = target_angle - turtle_pose_a.theta;

  // 将差值限制在-PI和PI之间
  while (delta_angle > PI) {
    delta_angle -= 2 * PI;
  }
  while (delta_angle < -PI) {
    delta_angle += 2 * PI;
  }

  // 控制乌龟A旋转
  geometry_msgs::Twist twist;
  twist.linear.x = 0;
  twist.angular.z = delta_angle > 0 ? ANGULAR_SPEED : -ANGULAR_SPEED;
  turtle_pub.publish(twist);

  // 控制乌龟A直行
  if (fabs(delta_angle) < 0.01) {
    twist.linear.x = LINEAR_SPEED;
    twist.angular.z = 0;
    turtle_pub.publish(twist);
  }
}

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

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

  // 创建乌龟A、B、C的位置订阅器
  ros::Subscriber turtle_pose_sub_a = nh.subscribe<turtlesim::Pose>('turtle1/pose', 10, turtlePoseCallbackA);
  ros::Subscriber turtle_pose_sub_b = nh.subscribe<turtlesim::Pose>('turtle2/pose', 10, turtlePoseCallbackB);
  ros::Subscriber turtle_pose_sub_c = nh.subscribe<turtlesim::Pose>('turtle3/pose', 10, turtlePoseCallbackC);

  // 创建乌龟A的运动控制器
  turtle_pub = nh.advertise<geometry_msgs::Twist>('turtle1/cmd_vel', 10);

  // 循环执行控制乌龟A运动的函数
  ros::Rate rate(10);
  while (ros::ok()) {
    controlTurtleA();
    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}

代码说明

初始化: 代码首先包含必要的头文件，定义常量和全局变量。
回调函数: turtlePoseCallbackA，turtlePoseCallbackB 和 turtlePoseCallbackC 分别用于接收来自三个乌龟 /turtle1/pose，/turtle2/pose，/turtle3/pose 的位置信息，并存储到全局变量中。
辅助函数: distance 函数计算两点之间的距离，angle 函数计算两点之间的角度。
控制逻辑: controlTurtleA 函数是控制乌龟 A 运动的核心逻辑：
- 首先，根据乌龟 A、B、C 的位置信息计算出它们之间的距离和角度。
- 然后，计算出乌龟 A 应该朝向的目标角度，并计算出当前角度与目标角度的差值。
- 最后，根据角度差值控制乌龟 A 进行旋转或直行运动。
主函数:
- 初始化 ROS 节点和节点句柄。
- 创建订阅器订阅三个乌龟的位置信息，并将回调函数与之关联。
- 创建发布器用于发布控制乌龟 A 运动的指令。
- 在主循环中，不断调用 controlTurtleA 函数控制乌龟 A 的运动。

注意

本代码假设乌龟 B 和 C 的初始位置已经固定，且不会发生变化。如果需要动态调整，需要修改代码逻辑。
本代码假设乌龟 A 一直向前直行，不会发生碰撞或越界。实际应用中需要考虑边界和碰撞检测。

总结

本教程介绍了如何使用 ROS C++ 编写代码，实现控制三只乌龟保持等边三角形编队运动。你可以根据自己的需求修改代码，实现更复杂的编队控制功能，例如：改变编队形状、动态调整队形等。