ROS入门21讲笔记（十）tf坐标系广播与监听的编程实现

广播是什么？

因为是入门，先不深究。可以借助TCP/IP中的广播地址(Broadcast Address)来理解一下，广播地址是 是专门用于同时向网络中（通常指同一子网）所有工作站进行发送的一个地址。tf坐标系的可以被所有节点看到的操作就叫广播

代码实现

#include 
#include 
#include 
#include 

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

    // /spawn是一个服务，用于产生海龟
    ros::service::waitForService("/spawn");

    //创建一个客户端，客户端应该准确给出你要的服务类型，如这里的/spawn
    ros::ServiceClient add_turtle=n.serviceClient("/spawn");
    //构造一个Spwan对象，然后call服务
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel=n.advertise("/turtle2/cmd_vel",10);
    tf::TransformListener listener;
    
    ros::Rate rate(10.0);
    while(n.ok())
    {
        tf::StampedTransform trans;
        try
        {
            listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),trans);
        }
        catch(const std::exception& e)
        {
            ROS_ERROR("%s",e.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z=4.0*atan2(trans.getOrigin().y(),trans.getOrigin().x());
        vel_msg.linear.x=0.5*sqrt(pow(trans.getOrigin().x(),2))+pow(trans.getOrigin().y(),2);
        turtle_vel.publish(vel_msg);

        rate.sleep();
        
    }
}

#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr &msg)
{
    //创建一个广播实例
    static tf::TransformBroadcaster br;

    //设置广播tf的数据
    tf::Transform trans;
        //设置位置
    trans.setOrigin(tf::Vector3(msg->x,msg->y,0.0));
    tf::Quaternion q;
        //设置姿态
    q.setRPY(0,0,msg->theta);
    trans.setRotation(q);

    //通过广播将数据广播出去，广播的内容StampedTransform 旋转矩阵+ 事件 + 坐标系名 + 乌龟名
    br.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"world",turtle_name));
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"my_tf_broadcaster");

    if(argc!=2)
    {
        ROS_ERROR("need turtle name argument");
        return -1;
    }

    turtle_name=argv[1];

    ros::NodeHandle n;
    //订阅一个乌龟的/pose信息，如果有订阅消息就会调用回调函数，pose信息是一直更新
    ros::Subscriber sub=n.subscribe(turtle_name+"/pose",10,&poseCallback);

    //spin 进入事件循环，一般在你订阅完之后调用
    ros::spin();

    return 0;
}

roscore
rosrun turtlesim turtlesim_node
rosrun  tfbroadcast_test broadcast __name:=turtle1_tf_broadcaster /turtle1
rosrun  tfbroadcast_test broadcast __name:=turtle2_tf_broadcaster /turtle2
rosrun tfbroadcast_test tf_listerner
rosrun turtlesim turtle_teleop_key