ROS CPP Turtle编队控制: 实现等边三角形编队

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

// 三只乌龟的初始位置和朝向 const float A_INIT_X = 2.0; const float A_INIT_Y = 2.0; const float A_INIT_THETA = 0.0; const float B_INIT_X = 2.0; const float B_INIT_Y = 3.0; const float B_INIT_THETA = 0.0; const float C_INIT_X = 2.0; const float C_INIT_Y = 4.0; const float C_INIT_THETA = 0.0;

// 三个乌龟的速度控制器 ros::Publisher turtleA_vel_pub; ros::Publisher turtleB_vel_pub; ros::Publisher turtleC_vel_pub;

// 三个乌龟的位置信息 turtlesim::Pose turtleA_pose; turtlesim::Pose turtleB_pose; turtlesim::Pose turtleC_pose;

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

// 控制A乌龟移动 void moveTurtleA() { // 计算A和B的距离 float distAB = distance(turtleA_pose.x, turtleA_pose.y, turtleB_pose.x, turtleB_pose.y); // 计算A和C的距离 float distAC = distance(turtleA_pose.x, turtleA_pose.y, turtleC_pose.x, turtleC_pose.y); // 计算B和C的距离 float distBC = distance(turtleB_pose.x, turtleB_pose.y, turtleC_pose.x, turtleC_pose.y);

// 计算三只乌龟应该移动的线速度和角速度
float linearVel = 1.0;
float angularVel = 0.0;
if (distAB > 1.0 || distAC > 1.0 || distBC > 1.0) {
    // 如果三只乌龟不在等边三角形上，则计算角速度
    float angleA = atan2(turtleB_pose.y - turtleA_pose.y, turtleB_pose.x - turtleA_pose.x);
    float angleB = atan2(turtleC_pose.y - turtleB_pose.y, turtleC_pose.x - turtleB_pose.x);
    float angleC = atan2(turtleA_pose.y - turtleC_pose.y, turtleA_pose.x - turtleC_pose.x);
    float angleDiffAB = angleB - angleA;
    float angleDiffBC = angleC - angleB;
    float angleDiffCA = angleA - angleC;
    if (angleDiffAB > M_PI) angleDiffAB -= 2 * M_PI;
    if (angleDiffAB < -M_PI) angleDiffAB += 2 * M_PI;
    if (angleDiffBC > M_PI) angleDiffBC -= 2 * M_PI;
    if (angleDiffBC < -M_PI) angleDiffBC += 2 * M_PI;
    if (angleDiffCA > M_PI) angleDiffCA -= 2 * M_PI;
    if (angleDiffCA < -M_PI) angleDiffCA += 2 * M_PI;
    angularVel = 1.0 * (angleDiffAB + angleDiffBC + angleDiffCA);
}

// 发布速度控制指令
geometry_msgs::Twist velMsg;
velMsg.linear.x = linearVel;
velMsg.angular.z = angularVel;
turtleA_vel_pub.publish(velMsg);

}

// 接收乌龟位置信息的回调函数 void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& msg) { turtleA_pose = *msg; moveTurtleA(); }

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

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

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

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

// 创建三只乌龟的速度控制器
turtleA_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
turtleB_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
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::ServiceClient spawnClient = nh.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn spawnA;
spawnA.request.x = A_INIT_X;
spawnA.request.y = A_INIT_Y;
spawnA.request.theta = A_INIT_THETA;
spawnA.request.name = "turtle1";
spawnClient.call(spawnA);
turtlesim::Spawn spawnB;
spawnB.request.x = B_INIT_X;
spawnB.request.y = B_INIT_Y;
spawnB.request.theta = B_INIT_THETA;
spawnB.request.name = "turtle2";
spawnClient.call(spawnB);
turtlesim::Spawn spawnC;
spawnC.request.x = C_INIT_X;
spawnC.request.y = C_INIT_Y;
spawnC.request.theta = C_INIT_THETA;
spawnC.request.name = "turtle3";
spawnClient.call(spawnC);

// 循环等待回调函数
ros::spin();

return 0;

}