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

本代码示例演示了如何使用 ROS C++ 代码实现三个乌龟 A、B、C 形成等边三角形编队，键盘控制 A 运动时，B 和 C 始终保持等边三角形形状，边长为 1，且朝向一致。

C++ 代码

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

// 定义全局变量
turtlesim::Pose turtleA_pose;
turtlesim::Pose turtleB_pose;
turtlesim::Pose turtleC_pose;

// 定义回调函数，更新乌龟 A 的位置
void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
  turtleA_pose = *msg;
}

// 定义回调函数，更新乌龟 B 的位置
void turtleBPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
  turtleB_pose = *msg;
}

// 定义回调函数，更新乌龟 C 的位置
void turtleCPoseCallback(const turtlesim::Pose::ConstPtr& msg)
{
  turtleC_pose = *msg;
}

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

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

  // 创建三只乌龟的速度控制器
  ros::Publisher turtleA_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
  ros::Publisher turtleB_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
  ros::Publisher turtleC_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 10);

  // 创建三只乌龟的位置订阅器
  ros::Subscriber turtleA_pose_sub = nh.subscribe("/turtle1/pose", 10, turtleAPoseCallback);
  ros::Subscriber turtleB_pose_sub = nh.subscribe("/turtle2/pose", 10, turtleBPoseCallback);
  ros::Subscriber turtleC_pose_sub = nh.subscribe("/turtle3/pose", 10, turtleCPoseCallback);

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

  // 等待乌龟 A、B、C 的位置信息
  while (ros::ok() && !(turtleA_pose.x > 0 && turtleB_pose.x > 0 && turtleC_pose.x > 0))
  {
    ros::spinOnce();
    loop_rate.sleep();
  }

  // 定义常量
  const double PI = 3.14159265358979323846;
  const double SIDE_LENGTH = 1.0;

  // 定义变量
  double angle_AB = 0.0;
  double angle_BC = 0.0;
  double angle_CA = 0.0;
  double distance_AB = 0.0;
  double distance_BC = 0.0;
  double distance_CA = 0.0;

  // 进入循环
  while (ros::ok())
  {
    // 计算三只乌龟之间的角度和距离
    angle_AB = atan2(turtleB_pose.y - turtleA_pose.y, turtleB_pose.x - turtleA_pose.x);
    angle_BC = atan2(turtleC_pose.y - turtleB_pose.y, turtleC_pose.x - turtleB_pose.x);
    angle_CA = atan2(turtleA_pose.y - turtleC_pose.y, turtleA_pose.x - turtleC_pose.x);
    distance_AB = sqrt(pow(turtleB_pose.y - turtleA_pose.y, 2) + pow(turtleB_pose.x - turtleA_pose.x, 2));
    distance_BC = sqrt(pow(turtleC_pose.y - turtleB_pose.y, 2) + pow(turtleC_pose.x - turtleB_pose.x, 2));
    distance_CA = sqrt(pow(turtleA_pose.y - turtleC_pose.y, 2) + pow(turtleA_pose.x - turtleC_pose.x, 2));

    // 控制乌龟 A 移动
    geometry_msgs::Twist turtleA_vel_msg;
    turtleA_vel_msg.linear.x = 1.0;
    turtleA_vel_msg.angular.z = 0.0;
    turtleA_vel_pub.publish(turtleA_vel_msg);

    // 控制乌龟 B 和 C 移动
    if (fabs(distance_AB - SIDE_LENGTH) > 0.1)
    {
      geometry_msgs::Twist turtleB_vel_msg;
      turtleB_vel_msg.linear.x = 0.5 * (distance_AB - SIDE_LENGTH);
      turtleB_vel_msg.angular.z = 2.0 * (angle_AB - turtleB_pose.theta);
      turtleB_vel_pub.publish(turtleB_vel_msg);

      geometry_msgs::Twist turtleC_vel_msg;
      turtleC_vel_msg.linear.x = 0.5 * (distance_BC - SIDE_LENGTH);
      turtleC_vel_msg.angular.z = 2.0 * (angle_BC - turtleC_pose.theta);
      turtleC_vel_pub.publish(turtleC_vel_msg);
    }
    else if (fabs(distance_BC - SIDE_LENGTH) > 0.1)
    {
      geometry_msgs::Twist turtleB_vel_msg;
      turtleB_vel_msg.linear.x = 0.5 * (distance_AB - SIDE_LENGTH);
      turtleB_vel_msg.angular.z = 2.0 * (angle_AB - turtleB_pose.theta);
      turtleB_vel_pub.publish(turtleB_vel_msg);

      geometry_msgs::Twist turtleC_vel_msg;
      turtleC_vel_msg.linear.x = 0.5 * (distance_CA - SIDE_LENGTH);
      turtleC_vel_msg.angular.z = 2.0 * (angle_CA - turtleC_pose.theta);
      turtleC_vel_pub.publish(turtleC_vel_msg);
    }
    else if (fabs(distance_CA - SIDE_LENGTH) > 0.1)
    {
      geometry_msgs::Twist turtleB_vel_msg;
      turtleB_vel_msg.linear.x = 0.5 * (distance_BC - SIDE_LENGTH);
      turtleB_vel_msg.angular.z = 2.0 * (angle_BC - turtleB_pose.theta);
      turtleB_vel_pub.publish(turtleB_vel_msg);

      geometry_msgs::Twist turtleC_vel_msg;
      turtleC_vel_msg.linear.x = 0.5 * (distance_CA - SIDE_LENGTH);
      turtleC_vel_msg.angular.z = 2.0 * (angle_CA - turtleC_pose.theta);
      turtleC_vel_pub.publish(turtleC_vel_msg);
    }
    else
    {
      geometry_msgs::Twist turtleB_vel_msg;
      turtleB_vel_msg.linear.x = 0.0;
      turtleB_vel_msg.angular.z = 0.0;
      turtleB_vel_pub.publish(turtleB_vel_msg);

      geometry_msgs::Twist turtleC_vel_msg;
      turtleC_vel_msg.linear.x = 0.0;
      turtleC_vel_msg.angular.z = 0.0;
      turtleC_vel_pub.publish(turtleC_vel_msg);
    }

    // 循环处理 ROS 消息
    ros::spinOnce();

    // 按照循环频率延时
    loop_rate.sleep();
  }

  return 0;
}

CMakeLists.txt 文件

cmake_minimum_required(VERSION 2.8.3)
project(turtle_triangle)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  turtlesim
  geometry_msgs
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES turtle_triangle
  CATKIN_DEPENDS roscpp turtlesim geometry_msgs
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(turtle_triangle src/turtle_triangle.cpp)

target_link_libraries(turtle_triangle
  ${catkin_LIBRARIES}
)

说明：

代码中使用了 turtlesim 包提供的乌龟仿真环境，以及 geometry_msgs 包提供的速度和位置信息。
乌龟 A 的运动由键盘控制，代码中的循环不断更新乌龟 B 和 C 的速度，使它们始终保持等边三角形形状。
CMakeLists.txt 文件指定了项目的依赖包和编译规则。

运行步骤：

在 ROS 环境中编译并运行该代码。
使用键盘控制乌龟 A 的运动，观察乌龟 B 和 C 的编队变化。

注意：

代码中的 SIDE_LENGTH 常量定义了等边三角形的边长。
代码中使用了 fabs() 函数来判断距离是否接近目标值。
代码中使用的角度和距离计算方法可以根据实际情况进行调整。

相关资源：

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

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

C++ 代码

CMakeLists.txt 文件