用ros编写代码实现产生两只乌龟A、B、C键盘控制A 运动时 A、B、C 会始终保持等边三角形的编队边 长为1且朝向一致我想要cpp代码

以下是使用ROS编写的cpp代码，实现产生两只乌龟A、B、C，键盘控制A运动时，A、B、C会始终保持等边三角形的编队，边长为1，且朝向一致。

#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;
}
``