ROS 乌龟编队控制：实现等边三角形编队

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

#define PI 3.14159265

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

turtlesim::Pose turtleA_pose; turtlesim::Pose turtleB_pose; turtlesim::Pose turtleC_pose;

void turtleAPoseCallback(const turtlesim::Pose::ConstPtr& pose_msg) { turtleA_pose = *pose_msg; }

void moveTurtleA(double speed, double distance, bool isForward) { geometry_msgs::Twist vel_msg; if(isForward) { vel_msg.linear.x = abs(speed); } else { vel_msg.linear.x = -abs(speed); } vel_msg.linear.y = 0; vel_msg.linear.z = 0; vel_msg.angular.x = 0; vel_msg.angular.y = 0; vel_msg.angular.z = 0;

double current_distance = 0.0;
ros::Rate loop_rate(10);

while(current_distance < distance)
{
    turtleA_pub.publish(vel_msg);
    double x_diff = turtleA_pose.x - turtleB_pose.x;
    double y_diff = turtleA_pose.y - turtleB_pose.y;
    double angle_diff = atan2(y_diff, x_diff) - turtleA_pose.theta;
    if(angle_diff > PI)
    {
        angle_diff -= 2 * PI;
    }
    else if(angle_diff < -PI)
    {
        angle_diff += 2 * PI;
    }
    vel_msg.angular.z = 4 * angle_diff;
    turtleB_pub.publish(vel_msg);
    turtleC_pub.publish(vel_msg);
    ros::spinOnce();
    loop_rate.sleep();
    current_distance = sqrt(pow(turtleA_pose.x - turtleB_pose.x, 2) + pow(turtleA_pose.y - turtleB_pose.y, 2));
}
vel_msg.linear.x = 0;
vel_msg.angular.z = 0;
turtleA_pub.publish(vel_msg);

}

int main(int argc, char **argv) { ros::init(argc, argv, 'turtle_triangular_formation'); ros::NodeHandle nh; turtleA_pub = nh.advertise<geometry_msgs::Twist>('turtle1/cmd_vel', 10); turtleB_pub = nh.advertise<geometry_msgs::Twist>('turtle2/cmd_vel', 10); turtleC_pub = nh.advertise<geometry_msgs::Twist>('turtle3/cmd_vel', 10); turtleA_sub = nh.subscribe('turtle1/pose', 10, turtleAPoseCallback); ros::Rate loop_rate(10);

double speed, distance;
bool isForward;

ROS_INFO('Enter speed: ');
std::cin >> speed;
ROS_INFO('Enter distance: ');
std::cin >> distance;
ROS_INFO('Forward? (1/0): ');
std::cin >> isForward;

moveTurtleA(speed, distance, isForward);

return 0;

}