ROS C++教程：实现三只乌龟的等边三角形编队控制

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

代码实现

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

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

#define PI 3.14159265358979323846

class TurtleController {
public:
    TurtleController(ros::NodeHandle& nh, std::string turtle_name) : nh_(nh), turtle_name_(turtle_name) {
        pose_sub_ = nh_.subscribe('/' + turtle_name_ + '/pose', 10, &TurtleController::poseCallback, this);
        vel_pub_ = nh_.advertise<geometry_msgs::Twist>('/' + turtle_name_ + '/cmd_vel', 10);
        cmd_vel_.linear.x = 0;
        cmd_vel_.linear.y = 0;
        cmd_vel_.linear.z = 0;
        cmd_vel_.angular.x = 0;
        cmd_vel_.angular.y = 0;
        cmd_vel_.angular.z = 0;
        target_pose_.x = 0;
        target_pose_.y = 0;
        target_pose_.theta = 0;
    }

    void poseCallback(const turtlesim::Pose::ConstPtr& pose_msg) {
        current_pose_ = *pose_msg;
    }

    void move(double linear_vel, double angular_vel) {
        cmd_vel_.linear.x = linear_vel;
        cmd_vel_.angular.z = angular_vel;
        vel_pub_.publish(cmd_vel_);
    }

    void stop() {
        cmd_vel_.linear.x = 0;
        cmd_vel_.angular.z = 0;
        vel_pub_.publish(cmd_vel_);
    }

    void setTargetPose(double x, double y, double theta) {
        target_pose_.x = x;
        target_pose_.y = y;
        target_pose_.theta = theta;
    }

    bool isAtTargetPose() {
        double distance = std::sqrt(std::pow(current_pose_.x - target_pose_.x, 2) + std::pow(current_pose_.y - target_pose_.y, 2));
        double angle = std::abs(current_pose_.theta - target_pose_.theta);
        if (angle > PI) {
            angle = 2 * PI - angle;
        }
        return distance < 0.01 && angle < 0.01;
    }

    void moveToTargetPose() {
        double linear_vel = 0.5;
        double angular_vel = 0.5;
        double distance = std::sqrt(std::pow(current_pose_.x - target_pose_.x, 2) + std::pow(current_pose_.y - target_pose_.y, 2));
        double angle = std::atan2(target_pose_.y - current_pose_.y, target_pose_.x - current_pose_.x) - current_pose_.theta;
        if (angle > PI) {
            angle -= 2 * PI;
        } else if (angle < -PI) {
            angle += 2 * PI;
        }
        move(linear_vel, angular_vel * angle);
    }

private:
    ros::NodeHandle nh_;
    std::string turtle_name_;
    turtlesim::Pose current_pose_;
    turtlesim::Pose target_pose_;
    ros::Subscriber pose_sub_;
    ros::Publisher vel_pub_;
    geometry_msgs::Twist cmd_vel_;
};

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

    TurtleController turtle_a(nh, 'turtle1');
    TurtleController turtle_b(nh, 'turtle2');
    TurtleController turtle_c(nh, 'turtle3');

    double side_length = 1;
    double angle = PI / 3;
    double x_a = 5;
    double y_a = 5;
    double x_b = x_a + side_length * std::cos(angle);
    double y_b = y_a + side_length * std::sin(angle);
    double x_c = x_a + side_length * std::cos(-angle);
    double y_c = y_a + side_length * std::sin(-angle);

    turtle_a.setTargetPose(x_a, y_a, 0);
    turtle_b.setTargetPose(x_b, y_b, angle);
    turtle_c.setTargetPose(x_c, y_c, -angle);

    ros::Rate loop_rate(10);

    while (ros::ok()) {
        if (turtle_a.isAtTargetPose()) {
            turtle_a.stop();
        } else {
            turtle_a.moveToTargetPose();
        }

        double angle_b = std::atan2(turtle_a.current_pose_.y - turtle_b.current_pose_.y, turtle_a.current_pose_.x - turtle_b.current_pose_.x);
        double angle_c = std::atan2(turtle_a.current_pose_.y - turtle_c.current_pose_.y, turtle_a.current_pose_.x - turtle_c.current_pose_.x);

        turtle_b.setTargetPose(turtle_a.current_pose_.x + side_length * std::cos(angle_b + angle), turtle_a.current_pose_.y + side_length * std::sin(angle_b + angle), angle_b + angle);
        turtle_c.setTargetPose(turtle_a.current_pose_.x + side_length * std::cos(angle_c - angle), turtle_a.current_pose_.y + side_length * std::sin(angle_c - angle), angle_c - angle);

        if (turtle_b.isAtTargetPose()) {
            turtle_b.stop();
        } else {
            turtle_b.moveToTargetPose();
        }

        if (turtle_c.isAtTargetPose()) {
            turtle_c.stop();
        } else {
            turtle_c.moveToTargetPose();
        }

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

    return 0;
}

代码解析

TurtleController类: 我们定义了一个 TurtleController 类来封装对乌龟的控制操作。
- 构造函数: 初始化节点句柄、乌龟名称、订阅乌龟位置信息、发布速度控制信息等。
- poseCallback 函数: 接收乌龟当前位置信息。
- move 函数: 控制乌龟移动。
- stop 函数: 停止乌龟移动。
- setTargetPose 函数: 设置乌龟的目标位置。
- isAtTargetPose 函数: 判断乌龟是否到达目标位置。
- moveToTargetPose 函数: 控制乌龟移动到目标位置。
main 函数:
- 初始化 ROS 节点。
- 创建三个 TurtleController 对象分别代表三只乌龟 A、B、C。
- 设置三只乌龟的初始位置，使它们形成等边三角形。
- 进入主循环，不断更新乌龟的位置和目标位置：
  - 控制乌龟A移动到目标位置。
  - 根据乌龟A的位置计算乌龟B和C的目标位置。
  - 控制乌龟B和C移动到目标位置。
- 使用 ros::spinOnce() 函数处理回调函数，使用 loop_rate.sleep() 函数控制循环频率。

总结

本教程介绍了如何使用 ROS C++ 编写代码，控制三只乌龟形成等边三角形编队，并通过键盘控制其中一只乌龟的运动，使编队保持形状和朝向一致。你可以根据自己的需要修改代码，实现更复杂的编队控制功能。