ROS学习第五天 ROS常用的组件库（一）——TF

灵性花火

已于 2022-10-10 18:24:02 修改

阅读量1.7k

点赞数 1

分类专栏： # ROS实践与实例文章标签：学习

于 2022-10-06 11:19:40 首次发布

本文链接：https://blog.youkuaiyun.com/y18855021710/article/details/127180798

版权

ROS实践与实例专栏收录该内容

8 篇文章

订阅专栏

题外话：
首先给大家道个歉，因为之前迷上了无人驾驶apollo仿真，花了一段时间研究。后来发现没有资源，没有设备，是真的难学，所以放弃了apollo。最近又到了开题答辩时间，花了老长时间和师兄互相折磨，可能还要耽误一段时间才能恢复更新。而且最近发现前面更新的感觉还是有些不够仔细，请放心，最后更完了会修改前面。马上要开嵌入式模块以及刷题模块，做一个欢快的学习仔。

感谢：

http://www.autolabor.com.cn/book/ROSTutorials/di-5-zhang-ji-qi-ren-dao-hang/51-tfzuo-biao-bian-huan.html

这篇博客大佬指出了tf还有一些可视化的命令

(51条消息) ROS中的常用组件（二）——TF坐标变换_我不是“耀”神的博客-优快云博客_echo tf

在ROS中有一些小组件库，这些组件是关于实际机器人开发头文件，目前常用的是

TF坐标变换

Rosbag

rqt 工具箱

5.1 TF坐标变换

如果你研究机器人的运动情况，那么TF坐标变换就一定要学习，因为这就是机器人运动相关的组件。就像某本书上说过：机器人的精密程度在一定情况下取决于传感器的精确程度，机器人运动就借助这些传感器进行的，传感器后面会提供一些使用情况，这里就不细说了。
我们现在已知ROS小车的某些传感器就能够提供这些信号，这些信号在运动学上表现得是欧拉角或者四元素。现在我们需要一个舞台来体现这种变换。

首先介绍的是头文件

geometry_msgs/TransformStamped 这个头文件实现的是坐标系和坐标系之间的关系，
tf2_ros/static_transform_broadcaster 用来广播上面坐标系之间的关系
tf2/LinearMath/Quaternion则是四元素和欧拉角之间的转换头文件
tf2_ros/transform_listener用来接受上面广播的tf关系

5.1.1实现初始坐标位置

首先要新建功能包，然后在功能包里面输入
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs

然后发送坐标之间的关系（只有发送出去才能在rviz和其他节点接收到）

/*

    静态坐标变换发布方:

        发布关于 laser 坐标系的位置信息

    实现流程:

        1.包含头文件

        2.初始化 ROS 节点

        3.创建静态坐标转换广播器

        4.创建坐标系信息

        5.广播器发布坐标系信息

        6.spin()

*/


// 1.包含头文件

#include "ros/ros.h"

#include "tf2_ros/static_transform_broadcaster.h"

#include "geometry_msgs/TransformStamped.h"

#include "tf2/LinearMath/Quaternion.h"



int main(int argc, char *argv[])

{

    setlocale(LC_ALL,"");

    // 2.初始化 ROS 节点

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

    // 3.创建静态坐标转换广播器

    tf2_ros::StaticTransformBroadcaster broadcaster;

    // 4.创建坐标系信息

    geometry_msgs::TransformStamped ts;

    //----设置头信息

    ts.header.seq = 100;

    ts.header.stamp = ros::Time::now();

    ts.header.frame_id = "base_link";//是坐标系的中心

    //----设置子级坐标系（备注1）

    ts.child_frame_id = "laser";

    //----设置子级相对于父级的偏移量

    ts.transform.translation.x = 0.2;

    ts.transform.translation.y = 0.0;

    ts.transform.translation.z = 0.5;

    //----设置四元数:将 欧拉角数据转换成四元数

    tf2::Quaternion qtn;

    qtn.setRPY(0,0,0);//这个对象能够使欧拉角转换为四元素

    ts.transform.rotation.x = qtn.getX();

    ts.transform.rotation.y = qtn.getY();

    ts.transform.rotation.z = qtn.getZ();

    ts.transform.rotation.w = qtn.getW();//欧拉角000-转换成四元素0001

    // 5.广播器发布坐标系信息

    broadcaster.sendTransform(ts);

    ros::spin();

    return 0;

}

备注1:这里的子坐标系其实就是参考中心坐标系的变换情况，上面的代码主要是设定一些基础的参数，小车中心坐标系在传感器（主坐标系）的某个位姿

可以运行roscore使用rostopic list以及rostopic echo /tf_static来查看

当然还可以通过图形化界面来查看

输入rviz图形化界面，设置fixed frame为著坐标系，点击add找到tf来查看

5.1.2实现接受信息

实现一个初始坐标的设定之后，其他的设备可能会需要接受这一设定信息（在雷达的世界坐标系到小车的坐标系上），接收方就按照坐标基础实现其坐标系之间的静态转换

发送方实现了（laser到base_link之间的变化情况），这里简单的介绍一下接收方的实现（实现某点在laser 上的坐标到base_link坐标）

#include "ros/ros.h"

#include "tf2_ros/transform_listener.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

#include "tf2/LinearMath/Quaternion.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"



int main(int argc, char *argv[])

{   setlocale(LC_ALL,"");

    // 2.初始化 ros 节点

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

    // 3.创建 ros 句柄

    ros::NodeHandle nh;

    // 4.创建 TF 订阅对象

    tf2_ros::Buffer buffer;

    tf2_ros::TransformListener listener(buffer);

    // 5.解析订阅信息中获取 在laser坐标系中的坐标信息在

    geometry_msgs::PointStamped ps;

    ps.header.frame_id = " laser ";

    ps.header.stamp = ros::Time::now();

    // 这里坐标点是写死的，但是实际是动态生成的

            ps.point.x = 2.0;

            ps.point.y = 3.0;

            ps.point.z = 5.0;   

            ros::Rate rate(10);



          ros::Duration(2).sleep();//休眠两秒



    ros::Rate r(10);//设置循环频率

    while (ros::ok())

    {

        geometry_msgs::PointStamped ps_out;

        ps_out=buffer.transform(ps," base_link ");

        // r.sleep();

        ROS_INFO("转换后x=%.2f,y=%.2f,z=%.2f ,%s",

        ps_out.point.x,

        ps_out.point.y,

        ps_out.point.z,

        ps_out.header.frame_id.c_str()

        );

        rate.sleep();

        ros::spinOnce();

    }

    return 0;

}

如果报以下错就是需要加延迟

5.1.3补充

在实际运动中不会简单是两个坐标系，往往是三个以上坐标系，这里就指出了在世界坐标系下有两个son坐标系，大致类型是一样的就不细说了

/*



需求:

    现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，

    son1 相对于 world，以及 son2 相对于 world 的关系是已知的，

    求 son1 与 son2的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

实现流程:

    1.包含头文件

    2.初始化 ros 节点

    3.创建 ros 句柄

    4.创建 TF 订阅对象

    5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标

      解析 son1 中的点相对于 son2 的坐标

    6.spin



*/

//1.包含头文件

#include "ros/ros.h"

#include "tf2_ros/transform_listener.h"

#include "tf2/LinearMath/Quaternion.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

#include "geometry_msgs/TransformStamped.h"

#include "geometry_msgs/PointStamped.h"



int main(int argc, char *argv[])

{  

    setlocale(LC_ALL,"");

    // 2.初始化 ros 节点

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

    // 3.创建 ros 句柄

    ros::NodeHandle nh;

    // 4.创建 TF 订阅对象

    tf2_ros::Buffer buffer;

    tf2_ros::TransformListener listener(buffer);

    // 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标

    ros::Rate r(1);

    while (ros::ok())

    {

        try

        {

        //   解析 son1 中的点相对于 son2 的坐标

            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));

            ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());

            ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());

            ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",

                    tfs.transform.translation.x,

                    tfs.transform.translation.y,

                    tfs.transform.translation.z

                    );



            // 坐标点解析

            geometry_msgs::PointStamped ps;

            ps.header.frame_id = "son1";

            ps.header.stamp = ros::Time::now();

            ps.point.x = 1.0;

            ps.point.y = 2.0;

            ps.point.z = 3.0;



            geometry_msgs::PointStamped psAtSon2;

            psAtSon2 = buffer.transform(ps,"son2");

            ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",

                    psAtSon2.point.x,

                    psAtSon2.point.y,

                    psAtSon2.point.z

                    );

        }

        catch(const std::exception& e)

        {

            // std::cerr << e.what() << '\n';

            ROS_INFO("异常信息:%s",e.what());

        }

        r.sleep();

        // 6.spin

        ros::spinOnce();

    }

    return 0;

}

5.1.4动态坐标变化

在上面只是简单的使用了静态坐标的方法，这里将要控制小乌龟实现动态坐标变换

一个问题是如何获取动物体，哪里有动物？：
前面说过小乌龟，可以用键盘控制小乌龟，然后获取运动信息

问题：怎么获取小乌龟的位姿信息

//在视频里说过：启动一个小品乌龟使用的是

//和虚拟键盘…，通过使用rostopic能够看到或获取小乌龟位姿是pose

//因此查看这个pose，也就是说小乌龟生成的时候就已经有一个世界坐标系了，小乌龟运动就是在这个坐标系下的运动

//使用rostopic info /turtle1/pose可以看到格式

是这种类型，所以rosmsg info …/…即

简单说一下：窗口的左下角是坐标原点，而这个pose则是小乌龟在原点上的坐标信息

新建功能包

tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

发送方：小乌龟坐标系在世界坐标系的坐标是pose，这个pose消息是时刻变化的，所以二者之间的关系不仅需要广播出去，还需要注意这个值是变化的

// 1.包含头文件

#include "ros/ros.h"

#include "turtlesim/Pose.h"

#include "tf2_ros/transform_broadcaster.h"

#include "geometry_msgs/TransformStamped.h"

#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){

    //  5-1.创建 TF 广播器

    static tf2_ros::TransformBroadcaster broadcaster;

    //  5-2.创建 广播的数据(通过 pose 设置)

    geometry_msgs::TransformStamped tfs;

    //  |----头设置

    tfs.header.frame_id = "world";

    tfs.header.stamp = ros::Time::now();



    //  |----坐标系 ID

    tfs.child_frame_id = "turtle1";



    //  |----坐标系相对信息设置

    tfs.transform.translation.x = pose->x;

    tfs.transform.translation.y = pose->y;

    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0

    //  |--------- 四元数设置

    tf2::Quaternion qtn;

    qtn.setRPY(0,0,pose->theta);//偏航角是pose的偏航角

    tfs.transform.rotation.x = qtn.getX();

    tfs.transform.rotation.y = qtn.getY();

    tfs.transform.rotation.z = qtn.getZ();

    tfs.transform.rotation.w = qtn.getW();





    //  5-3.广播器发布数据

    broadcaster.sendTransform(tfs);

}



int main(int argc, char *argv[])

{

    setlocale(LC_ALL,"");

    // 2.初始化 ROS 节点

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

    // 3.创建 ROS 句柄

    ros::NodeHandle nh;

    // 4.创建订阅对象

    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);

    //     5.回调函数处理订阅到的数据(实现TF广播)

    //       

    // 6.spin

    ros::spin();

    return 0;

}

接收方实现

#include "ros/ros.h"

#include "tf2_ros/transform_listener.h"

#include "tf2_ros/buffer.h"

#include "geometry_msgs/PointStamped.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件



int main(int argc, char *argv[])

{

    setlocale(LC_ALL,"");

    // 2.初始化 ROS 节点

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

    ros::NodeHandle nh;

    // 3.创建 TF 订阅节点

    tf2_ros::Buffer buffer;

    tf2_ros::TransformListener listener(buffer);



    ros::Rate r(1);

    while (ros::ok())

    {

    // 4. 子级坐标系生成一个坐标点

        geometry_msgs::PointStamped point_laser;

        point_laser.header.frame_id = "turtle1";

        point_laser.header.stamp = ros::Time();//这个地方有时间匹配问题

        point_laser.point.x = 1;

        point_laser.point.y = 1;

        point_laser.point.z = 0;

    // 5. 父级坐标系是转换得到的，因为我们无法得到窗口的原点

        //新建一个坐标点，用于接收转换结果 

        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------

        try

        {

            geometry_msgs::PointStamped point_base;

            point_base = buffer.transform(point_laser,"world");

            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);



        }

        catch(const std::exception& e)

        {

            // std::cerr << e.what() << '\n';

            ROS_INFO("程序异常:%s",e.what());

        }





        r.sleep(); 

        ros::spinOnce();

    }





    return 0;

}

在已经打开小乌龟的窗口下执行上面两个文件，并用键盘控制