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

本代码使用 ROS C++ 实现两只乌龟 A、B、C 保持等边三角形编队的控制，键盘控制乌龟 A 运动时，A、B、C 会始终保持边长为 1 的等边三角形编队，且朝向一致。

代码实现

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

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

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

// 回调函数，获取turtleA的位置信息
void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& pose_msg)
{
    turtleA_pose = *pose_msg;
}

// 回调函数，获取turtleB的位置信息
void turtleBPoseCallback(const turtlesim::Pose::ConstPtr& pose_msg)
{
    turtleB_pose = *pose_msg;
}

// 回调函数，获取turtleC的位置信息
void turtleCPoseCallback(const turtlesim::Pose::ConstPtr& pose_msg)
{
    turtleC_pose = *pose_msg;
}

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

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

    // 创建发布速度控制命令的Publisher
    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);

    // 创建订阅turtleA、turtleB、turtleC位置信息的Subscriber
    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);

    // 等待订阅者连接
    while (turtleA_pose_sub.getNumPublishers() == 0 || turtleB_pose_sub.getNumPublishers() == 0 || turtleC_pose_sub.getNumPublishers() == 0)
    {
        ROS_WARN_ONCE("Please create subscribers to /turtle1/pose, /turtle2/pose, /turtle3/pose");
        sleep(1);
    }

    // 设置控制频率
    ros::Rate rate(10);

    while (ros::ok())
    {
        // 计算turtleA当前朝向
        double theta = turtleA_pose.theta;

        // 计算turtleB、turtleC应该在的位置
        double xB = turtleA_pose.x + SIDE_LENGTH*cos(theta + 2*PI/3);
        double yB = turtleA_pose.y + SIDE_LENGTH*sin(theta + 2*PI/3);
        double xC = turtleA_pose.x + SIDE_LENGTH*cos(theta + 4*PI/3);
        double yC = turtleA_pose.y + SIDE_LENGTH*sin(theta + 4*PI/3);

        // 计算turtleB、turtleC与目标位置的距离
        double distB = distance(turtleB_pose.x, turtleB_pose.y, xB, yB);
        double distC = distance(turtleC_pose.x, turtleC_pose.y, xC, yC);

        // 如果距离过大，则需要调整位置
        if (distB > 0.1 || distC > 0.1)
        {
            // 计算turtleB的速度控制命令
            geometry_msgs::Twist velB;
            velB.linear.x = 2*(xB - turtleB_pose.x);
            velB.linear.y = 2*(yB - turtleB_pose.y);
            velB.angular.z = 4*(theta + 2*PI/3 - turtleB_pose.theta);
            turtleB_vel_pub.publish(velB);

            // 计算turtleC的速度控制命令
            geometry_msgs::Twist velC;
            velC.linear.x = 2*(xC - turtleC_pose.x);
            velC.linear.y = 2*(yC - turtleC_pose.y);
            velC.angular.z = 4*(theta + 4*PI/3 - turtleC_pose.theta);
            turtleC_vel_pub.publish(velC);
        }

        // 发布turtleA的速度控制命令
        geometry_msgs::Twist velA;
        velA.linear.x = 1.0;
        velA.angular.z = 0.0;
        turtleA_vel_pub.publish(velA);

        // 循环等待
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

运行方法

在终端中输入以下命令启动 turtlesim:

roscore
rosrun turtlesim turtlesim_node

在终端中输入以下命令启动三只乌龟:

rosrun turtlesim turtle_teleop_key
rosrun turtlesim turtle_teleop_key turtle:=/turtle2
rosrun turtlesim turtle_teleop_key turtle:=/turtle3

在另一个终端中编译并运行上面的代码:

cd <workspace>
catkin_make
source devel/setup.bash
rosrun <package_name> turtle_triangle

通过键盘控制 turtle1 的移动，turtle2 和 turtle3 会始终保持等边三角形的编队，边长为 1，朝向一致。

说明

代码首先定义了一些常量和全局变量，包括 PI、SIDE_LENGTH、乌龟的坐标信息等。
代码中创建了发布速度控制命令的 Publisher 和订阅乌龟位置信息的 Subscriber。
代码使用 turtleAPoseCallback、turtleBPoseCallback、turtleCPoseCallback 三个回调函数获取乌龟的位置信息。
代码使用 distance 函数计算两点之间的距离。
在主循环中，代码根据乌龟 A 的当前位置和朝向计算乌龟 B 和乌龟 C 应该在的位置，并发布控制命令，使其保持等边三角形编队。

注意事项

该代码仅适用于 turtlesim 环境。
由于代码中存在一些近似计算，乌龟的编队可能会出现轻微的偏差。
为了避免乌龟碰撞，需要调整代码中的控制参数。

其他

您可以根据自己的需求修改代码，例如改变编队形状、添加更多乌龟等。
您可以参考 ROS 官方文档和示例代码，学习更多有关 ROS 的知识。