多个机器人协作时，需要互相知道对方的姿态，利用turtlesim也可以进行多个乌龟的协作。

一、实现效果

某只乌龟的姿态信息：
x轴y轴、yaw角、线速度、角速度

二、文件部分

记得将cpp文件权限设置为可执行程序，并添加CMake编译规则，catkin_make，souce等一系列操作

三、代码部分

1、在功能包里创建新节点turtle_new.cpp 原理是向spawn这个服务提出请求，生出多个新乌龟

#include <ros/ros.h>
#include <turtlesim/Spawn.h>
 
int main(int argc, char** argv)
{
    int i;
    if (argc < 2)
    {
        ROS_ERROR("Turtle's name can't be empty");
        return -1;
    }
    ros::init(argc, argv, "turtle_new");
    ros::NodeHandle node;
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv_msg;
    for (i = 1; i < argc; ++i)
    {
        srv_msg.request.x = i;
        srv_msg.request.y = i;
        srv_msg.request.theta = 0;
        srv_msg.request.name = *(argv + i);
        add_turtle_client.call(srv_msg);
    }
    return 0;
}

2、广播tf坐标turtle_broadcaste.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
 
std::string turtle_name;
 
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion quaternion;
    
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    quaternion.setRPY(0, 0, msg->theta);
    transform.setRotation(quaternion);
 
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
 
int main(int argc, char** argv)
{

    ros::init(argc, argv, "turtle_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    }
    
    turtle_name = *(argv + 1); // = argv[1]
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
 
    ros::spin();
    
    return 0;
};

3、监听tf坐标turtle_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <string>
using namespace std;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtle_listener");
    ros::NodeHandle node;
    string follower_vel("/");
    if (argc != 3)
    {
        ROS_ERROR("Need two names of the turtles!");
        return -1;
    }
    follower_vel.append(argv[1]);
    follower_vel.append("/cmd_vel");
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(follower_vel, 10);
    tf::TransformListener listener;
 
    ros::Rate rate(10.0);

    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            listener.lookupTransform(argv[1], argv[2], ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
 
        geometry_msgs::Twist vel_msg;

        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);
 
        rate.sleep();
    }
    return 0;
};

4、新建launch文件follow.launch

<launch>
    <!-- 启动海龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 启动键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 创建五个乌龟节点 -->
    <node pkg="turtle_follow" type="turtle_new" args="A B C D E" name="turtle_new" />

    <!-- 创建六个乌龟的tf广播器 -->
    <node pkg="turtle_follow" type="turtle_broadcaster" args="A" name="turtle1_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="B" name="turtle2_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="C" name="turtle3_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="D" name="turtle4_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="E" name="turtle5_broadcaster" />
     <node pkg="turtle_follow" type="turtle_broadcaster" args="turtle1" name="turtle0_broadcaster" />
 
    <!-- 第一个参数的乌龟跟随第二个参数的乌龟 -->
    <node pkg="turtle_follow" type="turtle_listener" args="A turtle1" name="listener1" />
    <node pkg="turtle_follow" type="turtle_listener" args="B A" name="listener2" />
    <node pkg="turtle_follow" type="turtle_listener" args="C B" name="listener3" />
    <node pkg="turtle_follow" type="turtle_listener" args="D C" name="listener4" />
    <node pkg="turtle_follow" type="turtle_listener" args="E D" name="listener5" />
 </launch>