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

以下是实现代码，其中包含了键盘控制 A 运动的部分和保持编队的部分：

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

#define PI 3.14159265

ros::Publisher turtleA_pub, turtleB_pub, turtleC_pub;
ros::Subscriber turtleA_sub;

turtlesim::Pose turtleA_pose, turtleB_pose, turtleC_pose;

void turtleA_pose_callback(const turtlesim::Pose::ConstPtr& msg) {
    turtleA_pose = *msg;
}

void move_turtleA(double speed, double distance, bool isForward) {
    geometry_msgs::Twist turtleA_msg;
    turtleA_msg.linear.x = isForward ? speed : -speed;
    turtleA_msg.linear.y = 0;
    turtleA_msg.linear.z = 0;
    turtleA_msg.angular.x = 0;
    turtleA_msg.angular.y = 0;
    turtleA_msg.angular.z = 0;

    double t0 = ros::Time::now().toSec();
    double current_distance = 0;
    ros::Rate loop_rate(10);

    while (current_distance < distance) {
        turtleA_pub.publish(turtleA_msg);
        double t1 = ros::Time::now().toSec();
        current_distance = speed * (t1 - t0);
        ros::spinOnce();
        loop_rate.sleep();
    }

    turtleA_msg.linear.x = 0;
    turtleA_pub.publish(turtleA_msg);
}

void move_turtleB(double speed, double distance, bool isForward) {
    geometry_msgs::Twist turtleB_msg;
    turtleB_msg.linear.x = isForward ? speed : -speed;
    turtleB_msg.linear.y = 0;
    turtleB_msg.linear.z = 0;
    turtleB_msg.angular.x = 0;
    turtleB_msg.angular.y = 0;
    turtleB_msg.angular.z = 0;

    double t0 = ros::Time::now().toSec();
    double current_distance = 0;
    ros::Rate loop_rate(10);

    while (current_distance < distance) {
        turtleB_pub.publish(turtleB_msg);
        double t1 = ros::Time::now().toSec();
        current_distance = speed * (t1 - t0);
        ros::spinOnce();
        loop_rate.sleep();
    }

    turtleB_msg.linear.x = 0;
    turtleB_pub.publish(turtleB_msg);
}

void move_turtleC(double speed, double distance, bool isForward) {
    geometry_msgs::Twist turtleC_msg;
    turtleC_msg.linear.x = isForward ? speed : -speed;
    turtleC_msg.linear.y = 0;
    turtleC_msg.linear.z = 0;
    turtleC_msg.angular.x = 0;
    turtleC_msg.angular.y = 0;
    turtleC_msg.angular.z = 0;

    double t0 = ros::Time::now().toSec();
    double current_distance = 0;
    ros::Rate loop_rate(10);

    while (current_distance < distance) {
        turtleC_pub.publish(turtleC_msg);
        double t1 = ros::Time::now().toSec();
        current_distance = speed * (t1 - t0);
        ros::spinOnce();
        loop_rate.sleep();
    }

    turtleC_msg.linear.x = 0;
    turtleC_pub.publish(turtleC_msg);
}

void move_turtles(double speed, double distance, bool isForward) {
    move_turtleA(speed, distance, isForward);
    move_turtleB(speed, distance, isForward);
    move_turtleC(speed, distance, isForward);
}

void rotate_turtles(double angular_speed, double angle, bool clockwise) {
    geometry_msgs::Twist turtleA_msg, turtleB_msg, turtleC_msg;
    turtleA_msg.linear.x = 0;
    turtleA_msg.linear.y = 0;
    turtleA_msg.linear.z = 0;
    turtleA_msg.angular.x = 0;
    turtleA_msg.angular.y = 0;
    turtleA_msg.angular.z = clockwise ? -abs(angular_speed) : abs(angular_speed);

    turtleB_msg.linear.x = 0;
    turtleB_msg.linear.y = 0;
    turtleB_msg.linear.z = 0;
    turtleB_msg.angular.x = 0;
    turtleB_msg.angular.y = 0;
    turtleB_msg.angular.z = clockwise ? -abs(angular_speed) : abs(angular_speed);

    turtleC_msg.linear.x = 0;
    turtleC_msg.linear.y = 0;
    turtleC_msg.linear.z = 0;
    turtleC_msg.angular.x = 0;
    turtleC_msg.angular.y = 0;
    turtleC_msg.angular.z = clockwise ? -abs(angular_speed) : abs(angular_speed);

    double t0 = ros::Time::now().toSec();
    double current_angle = 0;
    ros::Rate loop_rate(10);

    while (current_angle < angle) {
        turtleA_pub.publish(turtleA_msg);
        turtleB_pub.publish(turtleB_msg);
        turtleC_pub.publish(turtleC_msg);
        double t1 = ros::Time::now().toSec();
        current_angle = angular_speed * (t1 - t0);
        ros::spinOnce();
        loop_rate.sleep();
    }

    turtleA_msg.angular.z = 0;
    turtleB_msg.angular.z = 0;
    turtleC_msg.angular.z = 0;
    turtleA_pub.publish(turtleA_msg);
    turtleB_pub.publish(turtleB_msg);
    turtleC_pub.publish(turtleC_msg);
}

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

    turtleA_pub = node.advertise<geometry_msgs::Twist>('/turtle1/cmd_vel', 10);
    turtleB_pub = node.advertise<geometry_msgs::Twist>('/turtle2/cmd_vel', 10);
    turtleC_pub = node.advertise<geometry_msgs::Twist>('/turtle3/cmd_vel', 10);
    turtleA_sub = node.subscribe('/turtle1/pose', 10, turtleA_pose_callback);

    ros::Rate loop_rate(10);

    // Move turtles to initial positions
    move_turtles(1.0, 2.0, true);
    rotate_turtles(PI/2, PI/2, false);
    move_turtles(1.0, 2.0, true);
    rotate_turtles(-PI/3, PI/3, true);

    while (ros::ok()) {
        // Get keyboard input
        char input;
        std::cin >> input;

        switch (input) {
            case 'w':
                move_turtleA(1.0, 1.0, true);
                move_turtles(1.0, sqrt(3), true);
                break;
            case 'a':
                rotate_turtles(PI/3, PI/3, true);
                move_turtleA(1.0, 1.0, true);
                move_turtles(1.0, sqrt(3), true);
                break;
            case 's':
                move_turtleA(1.0, 1.0, false);
                move_turtles(1.0, sqrt(3), false);
                break;
            case 'd':
                rotate_turtles(-PI/3, PI/3, false);
                move_turtleA(1.0, 1.0, true);
                move_turtles(1.0, sqrt(3), true);
                break;
            default:
                break;
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

在代码中，首先定义了三个 Publisher 和一个 Subscriber，分别用于控制三只乌龟的运动和获取 A 乌龟的位置信息。然后定义了一些函数，包括了移动和旋转三只乌龟的函数，以及移动三只乌龟到初始位置的函数。

在主函数中，首先移动三只乌龟到初始位置，然后进入循环中。循环中通过读取键盘输入来控制 A 乌龟的运动，然后调用移动和旋转函数来保持编队。循环中还包含了 ROS 的 spinOnce() 和 sleep() 函数，用于处理 ROS 的回调函数和控制运行频率。

需要注意的是，由于键盘输入的阻塞特性，需要在终端中按下回车键才能执行下一个键盘输入。