ROS C++ 代码实现乌龟三角形编队运动

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

// 三只乌龟的名称 const std::string TURTLE_A = "turtle1"; const std::string TURTLE_B = "turtle2"; const std::string TURTLE_C = "turtle3";

// 三角形边长 const double TRIANGLE_SIDE_LENGTH = 1.0;

// 两只乌龟之间的距离 const double DISTANCE_BETWEEN_TURTLES = TRIANGLE_SIDE_LENGTH / std::sqrt(3.0);

// 三只乌龟的初始位置 const double TURTLE_A_X = 1.0; const double TURTLE_A_Y = 1.0; const double TURTLE_B_X = TURTLE_A_X - DISTANCE_BETWEEN_TURTLES; const double TURTLE_B_Y = TURTLE_A_Y - TRIANGLE_SIDE_LENGTH / 2.0; const double TURTLE_C_X = TURTLE_A_X + DISTANCE_BETWEEN_TURTLES; const double TURTLE_C_Y = TURTLE_A_Y - TRIANGLE_SIDE_LENGTH / 2.0;

// 三只乌龟的速度控制器 ros::Publisher turtle_a_vel_pub; ros::Publisher turtle_b_vel_pub; ros::Publisher turtle_c_vel_pub;

// 三只乌龟的位置信息 turtlesim::Pose turtle_a_pose; turtlesim::Pose turtle_b_pose; turtlesim::Pose turtle_c_pose;

// 更新乌龟的位置信息 void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& msg) { turtle_a_pose = *msg; }

void turtleBPoseCallback(const turtlesim::Pose::ConstPtr& msg) { turtle_b_pose = *msg; }

void turtleCPoseCallback(const turtlesim::Pose::ConstPtr& msg) { turtle_c_pose = *msg; }

// 计算两只乌龟之间的距离 double distanceBetweenTurtles(const turtlesim::Pose& turtle1, const turtlesim::Pose& turtle2) { return std::sqrt(std::pow(turtle1.x - turtle2.x, 2.0) + std::pow(turtle1.y - turtle2.y, 2.0)); }

// 控制乌龟A运动 void controlTurtleA(double linear_vel, double angular_vel) { geometry_msgs::Twist vel_msg; vel_msg.linear.x = linear_vel; vel_msg.angular.z = angular_vel; turtle_a_vel_pub.publish(vel_msg); }

// 控制乌龟B运动 void controlTurtleB(double linear_vel, double angular_vel) { double distance_to_a = distanceBetweenTurtles(turtle_b_pose, turtle_a_pose); double angle_to_a = std::atan2(turtle_a_pose.y - turtle_b_pose.y, turtle_a_pose.x - turtle_b_pose.x);

double desired_distance = DISTANCE_BETWEEN_TURTLES;
double desired_angle = angle_to_a;

if (distance_to_a < DISTANCE_BETWEEN_TURTLES) {
    desired_distance = distance_to_a;
    desired_angle = std::atan2(turtle_a_pose.y - turtle_b_pose.y, turtle_a_pose.x - turtle_b_pose.x);
}

double linear_error = desired_distance - distance_to_a;
double angular_error = desired_angle - turtle_b_pose.theta;

geometry_msgs::Twist vel_msg;
vel_msg.linear.x = linear_vel * linear_error;
vel_msg.angular.z = angular_vel * angular_error;
turtle_b_vel_pub.publish(vel_msg);

}

// 控制乌龟C运动 void controlTurtleC(double linear_vel, double angular_vel) { double distance_to_a = distanceBetweenTurtles(turtle_c_pose, turtle_a_pose); double angle_to_a = std::atan2(turtle_a_pose.y - turtle_c_pose.y, turtle_a_pose.x - turtle_c_pose.x);

double desired_distance = DISTANCE_BETWEEN_TURTLES;
double desired_angle = angle_to_a;

if (distance_to_a < DISTANCE_BETWEEN_TURTLES) {
    desired_distance = distance_to_a;
    desired_angle = std::atan2(turtle_a_pose.y - turtle_c_pose.y, turtle_a_pose.x - turtle_c_pose.x);
}

double linear_error = desired_distance - distance_to_a;
double angular_error = desired_angle - turtle_c_pose.theta;

geometry_msgs::Twist vel_msg;
vel_msg.linear.x = linear_vel * linear_error;
vel_msg.angular.z = angular_vel * angular_error;
turtle_c_vel_pub.publish(vel_msg);

}

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

// 初始化速度控制器
turtle_a_vel_pub = nh.advertise<geometry_msgs::Twist>(TURTLE_A + "/cmd_vel", 10);
turtle_b_vel_pub = nh.advertise<geometry_msgs::Twist>(TURTLE_B + "/cmd_vel", 10);
turtle_c_vel_pub = nh.advertise<geometry_msgs::Twist>(TURTLE_C + "/cmd_vel", 10);

// 订阅三只乌龟的位置信息
ros::Subscriber turtle_a_pose_sub = nh.subscribe(TURTLE_A + "/pose", 10, turtleAPoseCallback);
ros::Subscriber turtle_b_pose_sub = nh.subscribe(TURTLE_B + "/pose", 10, turtleBPoseCallback);
ros::Subscriber turtle_c_pose_sub = nh.subscribe(TURTLE_C + "/pose", 10, turtleCPoseCallback);

// 设置三只乌龟的初始位置
controlTurtleA(0.0, 0.0);
controlTurtleB(0.0, 0.0);
controlTurtleC(0.0, 0.0);
ros::Duration(1.0).sleep();
controlTurtleA(0.5, 0.0);
controlTurtleB(0.5, 0.0);
controlTurtleC(0.5, 0.0);
ros::Duration(1.0).sleep();
controlTurtleA(0.0, 0.0);
controlTurtleB(0.0, 0.0);
controlTurtleC(0.0, 0.0);

// 循环读取键盘输入，控制乌龟A的运动
while (ros::ok()) {
    char c;
    std::cin >> c;

    double linear_vel = 0.5;
    double angular_vel = 0.5;

    switch (c) {
        case 'w':
            controlTurtleA(linear_vel, 0.0);
            controlTurtleB(linear_vel, 0.0);
            controlTurtleC(linear_vel, 0.0);
            break;
        case 'a':
            controlTurtleA(0.0, angular_vel);
            controlTurtleB(0.0, angular_vel);
            controlTurtleC(0.0, angular_vel);
            break;
        case 's':
            controlTurtleA(-linear_vel, 0.0);
            controlTurtleB(-linear_vel, 0.0);
            controlTurtleC(-linear_vel, 0.0);
            break;
        case 'd':
            controlTurtleA(0.0, -angular_vel);
            controlTurtleB(0.0, -angular_vel);
            controlTurtleC(0.0, -angular_vel);
            break;
        case 'q':
            return 0;
        default:
            controlTurtleA(0.0, 0.0);
            controlTurtleB(0.0, 0.0);
            controlTurtleC(0.0, 0.0);
            break;
    }
}

return 0;

}