25. tf坐标系广播与监听的编程实现

pkg需要的依赖：roscpp rospy tf turtlesim(例子)

 

1. broadcaster（得到坐标系关系并发布到树中）

1.1 头文件部分：

#include <ros/ros.h>

#include <tf/transform_broadcaster.h>

#include <turtlesim/Pose.h>      海龟的位置是通过turtlesim不断往外发布的

 

1.2 main函数部分：

int main(int argc, char** argv)

{

    // 初始化ROS节点

    ros::init(argc, argv, "my_tf_broadcaster");

    // 输入参数作为海龟的名字，这是一个模板，就输入的是turtle1那么就会自动建立t1与world的关系，要是是输入t2，那么就会自动建立t2与world的关系。

    if (argc != 2)

    {

    ROS_ERROR("need turtle name as argument");

    return -1;

    }

    turtle_name = argv[1];

    // 订阅海龟的位姿话题

    ros::NodeHandle node;

    ros::Subscriber sub = node.subscribe(turtle_name+"/pose",    10, &poseCallback);

    // 循环等待回调函数，一有参数进入就跳转至callback，callback是重点

    ros::spin();

    return 0;

};

 

1.3 Callback：

void poseCallback(const turtlesim::PoseConstPtr& msg)

# callback中传入的参数是海龟的位置，turtlesim::PoseConstPtr，海龟在整个坐标系下的坐标x，y，theta的位置内容。

{

    // 创建tf的广播器，创建一个实例

    static tf::TransformBroadcaster br;

    // 初始化tf数据，比如turtle1与world之间存在两个关系，平移与旋转，所以这两个数据组成了一个坐标变换transform

    tf::Transform transform;

    # 首先是平移，通过setOrigin(transform中的函数)设置平移参数。z永远为0,因为在平面

    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

    tf::Quaternion q;

    # 通过四元数来设置旋转，x与y方向没有旋转，只有z方向有旋转。

    q.setRPY(0, 0, msg->theta);

    transform.setRotation(q);  # 通过setrotation方法将旋转信息保存进入transform里面去

    // 广播world与海龟坐标系之间的tf数据。到这一步我们得到了两个坐标系之间的位置关系就可以完全描述出来了(turtle1与world之间关系)

    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

    // 通过br广播出去后，ros后台就会把该两个坐标系的位置关系插入到树中。广播出去的内容有：广播了transform(两个坐标系关系)，是哪个时间的关系，两个坐标系的名字。通过这一段程序后，树中就会出现world和turtle坐标系了

}

 

1.4 总结

 

2. listner：

2.1 头文件：

#include <ros/ros.h>

#include <tf/transform_listener.h>  监听器

#include <geometry_msgs/Twist.h>   要让turtle2动起来，所以有一个发布指令

#include <turtlesim/Spawn.h>   turtle2这个海龟是通过spawn产生的

 

2.2 main函数

int main(int argc, char** argv)

{

    // 初始化ROS节点

    ros::init(argc, argv, "my_tf_listener");

        // 创建节点句柄

    ros::NodeHandle node;

    // 请求产生turtle2，通过spawn方法产生一个新的海龟

    ros::service::waitForService("/spawn");

    ros::ServiceClient add_turtle =     node.serviceClient<turtlesim::Spawn>("/spawn");

    turtlesim::Spawn srv;

    add_turtle.call(srv);

    // 创建发布turtle2速度控制指令的发布者，要让海龟2动起来的话，需要创建一个速度发布者，发布一个话题给海龟2,里面数值为twist。

    ros::Publisher turtle_vel =     node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

    // 创建tf的监听器

    tf::TransformListener listener;

    ros::Rate rate(10.0);    // 频率

    while (node.ok())

        {

            // 获取turtle1与turtle2坐标系之间的tf数据，创建一个transform来保存平移与旋转的变化

            tf::StampedTransform transform;

        // 接下来两句是关键，如何通过listner来监听两个坐标系中的关系。一个是等待变化waitForTransform，一个是查询变换lookupTransform。

            try

            {

                // 坐标系中是否存在turtle1与turtle2这两个坐标系，存在的话就会跳过该语句继续下一个。时间为当前时间0,查询实时数据。等待时间为3秒，超过3秒则会报错

                listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0),     ros::Duration(3.0));

                // t1 与 t2 之间的关系，结果保存在transform里面。transform里面保存了两个坐标系之间的关系，也就是那个代表性的 ¨向量¨

                listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

            }

            catch (tf::TransformException &ex)

            {

                ROS_ERROR("%s",ex.what());

                ros::Duration(1.0).sleep();

                continue;

            }

            // 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令

            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;

};

海龟1与海龟2之间的向量，两者坐标系不断在移动，变化，所以角速度与线速度程序也在不断再被重复执行更新速度。

以上就是程序的底层原理。