5 ROS常用组件_geometry_msgs/vector4

第 5 章 ROS常用组件

在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件:

• TF坐标变换，实现不同类型的坐标系之间的转换；
• rosbag 用于录制ROS节点的执行过程并可以重放该过程；
• rqt 工具箱，集成了多款图形化的调试工具。

本章预期达成的学习目标:

• 了解 TF 坐标变换的概念以及应用场景；
• 能够独立完成TF案例:小乌龟跟随；
• 可以使用 rosbag 命令或编码的形式实现录制与回放；
• 能够熟练使用rqt中的图形化工具。

案例演示: 小乌龟跟随实现，该案例是ros中内置案例，终端下键入启动命令

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch`或`roslaunch turtle_tf2 turtle_tf2_demo.launch
1

键盘可以控制一只乌龟运动，另一只跟随运动。

5.1 TF坐标变换

机器人系统上，有多个传感器，如激光雷达、摄像头等，有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标，横向、纵向、高度的距离信息)的，以协助机器人定位障碍物，可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息吗？显示是不行的，这中间需要一个转换过程。更具体描述如下:

场景1:雷达与小车

现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？

场景2:现有一带机械臂的机器人(比如:PR2)需要夹取目标物，当前机器人头部摄像头可以探测到目标物的坐标(x,y,z)，不过该坐标是以摄像头为参考系的，而实际操作目标物的是机械臂的夹具，当前我们需要将该坐标转换成相对于机械臂夹具的坐标，这个过程如何实现？

当然，根据我们高中学习的知识，在明确了不同坐标系之间的的相对关系，就可以实现任何坐标点在不同坐标系之间的转换，但是该计算实现是较为常用的，且算法也有点复杂，因此在 ROS 中直接封装了相关的模块: 坐标变换(TF)。

---

概念

**tf:**TransForm Frame,坐标变换

**坐标系:**ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。

作用

在 ROS 中用于实现不同坐标系之间的点或向量的转换。

案例

**小乌龟跟随案例：**如本章引言部分演示。

说明

在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:

tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

tf2: 封装了坐标变换的常用消息。

tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

---

另请参考:

• http://wiki.ros.org/tf2

5.1.1 坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped

前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

1.geometry_msgs/TransformStamped

命令行键入:rosmsg info geometry_msgs/TransformStamped

std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                           #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

四元数用于表示坐标的相对姿态

2.geometry_msgs/PointStamped

命令行键入:rosmsg info geometry_msgs/PointStamped

std_msgs/Header header                    #头
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                           #|-- 所属坐标系的 id
geometry_msgs/Point point                 #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z
1
2
3
4
5
6
7
8

---

另请参考:

• http://docs.ros.org/en/api/geometry_msgs/html/msg/TransformStamped.html
• http://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html

5.1.2 静态坐标变换

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。

需求描述:

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

结果演示:

实现分析:

1. 坐标系相对关系，可以通过发布方发布
2. 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

**实现流程:**C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 编写发布方实现
3. 编写订阅方实现
4. 执行并查看结果

---

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

/* 
    需求：发布两个坐标系的相对关系

    流程：
        1.包含头文件
        2.设置编码 节点初始化 NodeHandle;
        3.创建发布对象；
        4.组织被发布的消息；
        5.发布数据；
        6.spin();

*/

int main(int argc, char *argv[])
{
    // 2.设置编码 节点初始化 NodeHandle;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh;
    // 3.创建发布对象；
    tf2_ros::StaticTransformBroadcaster pub;  // 创建静态坐标转换广播器
    // 4.组织被发布的消息；
    geometry_msgs::TransformStamped tfs; 
     //----设置头信息 
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link"; //相对坐标系中被参考的那一个，父级
     //----设置子级坐标系
    tfs.child_frame_id = "laser";
     //----设置子级相对于父级的偏移量
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    //需要根据欧拉角转换
    tf2::Quaternion qtn;  //创建 四元数 对象
    //向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数
    qtn.setRPY(0,0,0);  //欧拉角的单位是弧度
    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.发布数据；
    pub.sendTransform(tfs);
    // 6.spin();
    ros::spin();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

3.订阅方

#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"

/*
    订阅方：订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换

    流程：
        1.包含头文件
        2.编码，初始化，Nodehandle(必须的)；
        3.创建订阅对象； ---->订阅坐标系相对关系
        4.组织一个坐标点数据；
        5.转换算法。需要调用TF内置实现
        6.最后输出。

*/

int main(int argc, char *argv[])
{
    // 2.编码，初始化，Nodehandle(必须的)；
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 3.创建订阅对象； ---->订阅坐标系相对关系
    // 3.1 创建一个buffer缓存
    tf2_ros::Buffer buffer;
    // 3.2 再创建监听对象（监听对象可以将订阅的数据存入buffer）
    tf2_ros::TransformListener listener(buffer);
    // 4.组织一个（障碍）坐标点数据；(相对于子级坐标系)
    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::Duration(2).sleep();
    // 5.转换算法(相对于父级坐标系)。需要调用TF内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
        //核心代码 --- 将 ps 转换成相对于 base_link 的坐标点
        geometry_msgs::PointStamped ps_out; //新建一个坐标点，用于接收转换结果  
        /*  
            调用了 buffer 的转换函数 transform
            参数1：被转移的坐标点
            参数2：目标坐标系
            返回值：输出坐标点

            ps:调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            ps:运行时存在的问题：抛出一个异常 base_link 不存在
                原因：订阅数据是个耗时操作，可能调用 transform 转换函数时，坐标系的相对关系和还没有订阅到，因此出现异常
                解决：
                    方案1：在调用转换函数前，执行休眠
                    方案2：进行异常处理（建议）
        */

        try
        {
            ps_out = buffer.transform(ps,"base_link");
        // 6.最后输出。
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
            ps_out.point.x,
            ps_out.point.y,
            ps_out.point.z,
            ps_out.header.frame_id.c_str()
            );
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
} 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。

roscore

cd demo04_ws/
source ./devel/setup.bash
rosrun tf01_static demo01_static_pub 

source ./devel/setup.bash
rosrun tf01_static demo01_static_sub 

1
2
3
4
5
6
7
8
9

补充1:

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
1

示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser

也建议使用该种方式直接实现静态坐标系相对信息发布。

补充2:

可以借助于rviz显示坐标系关系，具体操作:

• 新建窗口输入命令:rviz;
• 在启动的 rviz 中设置Fixed Frame 为 base_link;
• 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系。

---

另请参考:

• [http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28C%2B%2B%29](http://wiki.ros.org/tf2/Tutorials/Writing a tf2 static broadcaster (C%2B%2B))
• [http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28Python%29](http://wiki.ros.org/tf2/Tutorials/Writing a tf2 static broadcaster (Python))
• [http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29](http://wiki.ros.org/tf2/Tutorials/Writing a tf2 listener (C%2B%2B))
• [http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28Python%29](http://wiki.ros.org/tf2/Tutorials/Writing a tf2 listener (Python))

5.1.3 动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述:

启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

结果演示:

实现分析:

1. 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2. 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3. 将 pose 信息转换成 坐标系相对信息并发布

**实现流程:**C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
3. 创建坐标相对关系订阅方
4. 执行

---

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"   //这个用来发布动态的
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"   //欧拉角相关的

/* 
    发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准备：
        话题：/turtle/pose
        消息：/turtlesim/Pose

    流程：
        1.包含头文件
        2.设置编码 初始化 NodeHandle;
        3.创建订阅对象，订阅 /turtle1/pose;
        4.回调函数处理订阅的消息：将位姿信息转换为坐标相对关系并发布（关注）
        5.spin()
*/

void doPose(const turtlesim::Pose::ConstPtr &pose){
    //获取位姿信息，转换成坐标系相对关系（核心），并发布
    //a. 创建发布对象；
    static tf2_ros::TransformBroadcaster pub;  //采用静态，每次访问的都是这个对象
    //b.组织被发布的数据；
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    //坐标系偏移量设置
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    /*  
        位姿信息中没有四元数，但是有个偏航角度，又知乌龟是 2D ，没有翻滚与俯仰角，
        所以可得出乌龟的欧拉角： 0 0 theta
    */
   tf2::Quaternion qtn;
   qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    //c.发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    // 2.设置编码 初始化 NodeHandle;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    // 3.创建订阅对象，订阅 /turtle1/pose;
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    // 4.回调函数处理订阅的消息：将位姿信息转换为坐标相对关系并发布（关注）
    // 5.spin()
    ros::spin();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

配置文件此处略。

3.订阅方

#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"

/*
    订阅方：订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换

    流程：
        1.包含头文件
        2.编码，初始化，Nodehandle(必须的)；
        3.创建订阅对象； ---->订阅坐标系相对关系
        4.组织一个坐标点数据；
        5.转换算法。需要调用TF内置实现
        6.最后输出。

*/

int main(int argc, char *argv[])
{
    // 2.编码，初始化，Nodehandle(必须的)；
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 3.创建订阅对象； ---->订阅坐标系相对关系
    // 3.1 创建一个buffer缓存
    tf2_ros::Buffer buffer;
    // 3.2 再创建监听对象（监听对象可以将订阅的数据存入buffer）
    tf2_ros::TransformListener listener(buffer);
    // 4.组织一个（障碍）坐标点数据；(相对于子级坐标系)
    geometry_msgs::PointStamped ps;
    //参考的坐标系
    ps.header.frame_id = "turtle1";
    //时间戳
    ps.header.stamp = ros::Time(0.0);
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    // 添加休眠
    // ros::Duration(2).sleep();
    // 5.转换算法(相对于父级坐标系)。需要调用TF内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
        //核心代码 --- 将 ps 转换成相对于 base_link 的坐标点
        geometry_msgs::PointStamped ps_out; //新建一个坐标点，用于接收转换结果  
        /*  
            调用了 buffer 的转换函数 transform
            参数1：被转移的坐标点
            参数2：目标坐标系
            返回值：输出坐标点

            ps:调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            ps:运行时存在的问题：抛出一个异常 base_link 不存在
                原因：订阅数据是个耗时操作，可能调用 transform 转换函数时，坐标系的相对关系和还没有订阅到，因此出现异常
                解决：
                    方案1：在调用转换函数前，执行休眠
                    方案2：进行异常处理（建议）
        */

        try
        {
            ps_out = buffer.transform(ps,"world");
        // 6.最后输出。
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
            ps_out.point.x,
            ps_out.point.y,
            ps_out.point.z,
            ps_out.header.frame_id.c_str()
            );
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
} 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84

5.1.4 多坐标变换

需求描述:

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

实现分析:

1. 首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
2. 然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
3. 最后，还要实现坐标点的转换

**实现流程:**C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 创建坐标相对关系发布方(需要发布两个坐标相对关系)
3. 创建坐标相对关系订阅方
4. 执行

---

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

为了方便，使用静态坐标变换发布

建立一个launch文件

<launch>
    <!-- 发布 son1 相对于 world 以及 son2 相对于 world 的坐标关系 -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 0 0 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="3 0 0 0 0 0 /world /son2" output="screen" />

</launch>
1
2
3
4
5
6

3.订阅方

/*
需求:
    现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
    son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
    求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
*/

#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"
#include "geometry_msgs/TransformStamped.h"

/* 
    订阅方实现：1.计算son1与son2的相对关系 2.计算son1的中某个坐标点在 son2 中的坐标值

    流程：
        1.包含头文件
        2.编码 初始化 NodeHandle;
        3.创建订阅对象；
        4.编写解析逻辑；
        5.spinOnce()；
*/

int main(int argc, char *argv[])
{
    // 2.编码 初始化 NodeHandle;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    // 3.创建订阅对象；
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    // 4.编写解析逻辑；

    // 创建坐标点
    geometry_msgs::PointStamped psAtSon1;

    psAtSon1.header.stamp = ros::Time::now();
    psAtSon1.header.frame_id = "son1";
    psAtSon1.point.x = 1.0;
    psAtSon1.point.y = 2.0;
    psAtSon1.point.z = 3.0;

    ros::Rate rate(10);
    while (ros::ok())
    {
        //核心
        try
        {
            //1.计算son1与son2的相对关系
            /* 
                A 相对于 B 的坐标系关系

                参数1：目标坐标系
                参数2：源坐标系
                参数3：ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系
                返回值：geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("son1 相对于 son2的信息: 父级:%s,子级:%s 偏移量(%.2f,%.2f,%.2f)",
                    son1ToSon2.header.frame_id.c_str(),   //son2
                    son1ToSon2.child_frame_id.c_str(),    //son1
                    son1ToSon2.transform.translation.x,
                    son1ToSon2.transform.translation.y,
                    son1ToSon2.transform.translation.z);  
            
            //2.计算son1的中某个坐标点在 son2 中的坐标值
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");
            ROS_INFO("坐标点在 son2 中的值(%.2f,%.2f,%.2f)",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    // 5.spinOnce()；
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，将输出换算后的结果。

5.1.5 坐标系关系查看

在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

1准备

首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:

sudo apt install ros-noetic-tf2-tools
1

2使用

2.1生成 pdf 文件

启动坐标系广播程序之后，运行如下命令:

rosrun tf2_tools view_frames.py
1

会产生类似于下面的日志信息:

[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...
1
2

查看当前目录会生成一个 frames.pdf 文件

2.2查看文件

可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf

5.1.6 TF坐标变换实操

需求描述:

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

结果演示:

实现分析:

乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

1. 启动乌龟显示节点
2. 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
3. 编写两只乌龟发布坐标信息的节点
4. 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

**实现流程:**C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 编写服务客户端，用于生成一只新的乌龟
3. 编写发布方，发布两只乌龟的坐标信息
4. 编写订阅方，订阅两只乌龟信息，生成速度信息并发布
5. 运行

准备工作:

1.了解如何创建第二只乌龟，且不受键盘控制

创建第二只乌龟需要使用rosservice,话题使用的是 spawn

rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'" 
name: "turtle_flow"
1
2
3
4
5

键盘是无法控制第二只乌龟运动的，因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息

2.了解如何获取两只乌龟的坐标

是通过话题 /乌龟名称/pose 来获取的

x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度
1
2
3
4
5

---

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.服务客户端(生成乌龟)

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

/* 
    需求：是向服务端发送请求，生成一只新的乌龟
        话题：/spawn
        消息：turtlesim/Spawn  
    
    1.包含头文件
    2.初始化ROS节点
    3.创建节点句柄
    4.创建客户端
    5.组织数据并发送
    6.处理响应
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化ROS节点
    ros::init(argc,argv,"service_call");
    // 3.创建节点句柄
    ros::NodeHandle nh;
    // 4.创建客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 5.组织数据并发送
    //5.1 组织请求数据
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "turtle2";
    //5.2 发送请求
    //判断服务器状态
    //ros::service::waitForService("/spawn");
    client.waitForExistence();
    bool flag = client.call(spawn);  //flag 接收响应状态，响应结果也会被设置进spawn对象
    // 6.处理响应
    if  (flag){
        ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());
    }else{
        ROS_INFO("请求失败!!!!");
    }
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

3.发布方(发布两只乌龟的坐标信息)

可以订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是订阅的话题名称，生成的坐标信息等稍有差异，可以将差异部分通过参数传入:

• 该节点需要启动两次
• 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建 ros 句柄
        5.创建订阅对象
        6.回调函数处理订阅的 pose 信息
            6-1.创建 TF 广播器
            6-2.将 pose 信息转换成 TransFormStamped
            6-3.发布
        7.spin

*/

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"   //这个用来发布动态的
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"   //欧拉角相关的

/* 
    发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准备：
        话题：/turtle/pose
        消息：/turtlesim/Pose

    流程：
        1.包含头文件
        2.设置编码 初始化 NodeHandle;
        3.创建订阅对象，订阅 /turtle1/pose;
        4.回调函数处理订阅的消息：将位姿信息转换为坐标相对关系并发布（关注）
        5.spin()
*/

//声明变量接收传递的参数
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr &pose){
    //获取位姿信息，转换成坐标系相对关系（核心），并发布
    //a. 创建发布对象；
    static tf2_ros::TransformBroadcaster pub;  //采用静态，每次访问的都是这个对象
    //b.组织被发布的数据；
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    // 关键点2：动态传入
    //ts.child_frame_id = "turtle1";
    ts.child_frame_id = turtle_name;
    //坐标系偏移量设置
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    /*  
        位姿信息中没有四元数，但是有个偏航角度，又知乌龟是 2D ，没有翻滚与俯仰角，
        所以可得出乌龟的欧拉角： 0 0 theta
    */
   tf2::Quaternion qtn;
   qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    //c.发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    // 2.设置编码 初始化 NodeHandle;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    /*  
        解析 launch 文件通过 args 传入的参数
    */
   if (argc != 2)
   {
       ROS_ERROR("请传入一个参数");
       return 1;
   }else{
       turtle_name = argv[1];
   }
   
    
    // 3.创建订阅对象，订阅 /turtle1/pose;
    // 关键点1：订阅的话题名称，turtle1 或 turtle2 动态传入
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose);
    // 4.回调函数处理订阅的消息：将位姿信息转换为坐标相对关系并发布（关注）
    // 5.spin()
    ros::spin();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

4.订阅方(解析坐标信息并生成速度信息)

#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"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"


/* 
    需求1：换算出 turtle1 相对于 turtle2 的关系
    需求2：计算角速度和线速度并发布

*/

int main(int argc, char *argv[])
{
    // 2.编码 初始化 NodeHandle;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    // 3.创建订阅对象；
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    
    //A.创建发布对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);

    // 4.编写解析逻辑；
    ros::Rate rate(10);
    while (ros::ok())
    {
        //核心
        try
        {
            //1.计算son1与son2的相对关系
            /* 
                A 相对于 B 的坐标系关系

                参数1：目标坐标系
                参数2：源坐标系
                参数3：ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系
                返回值：geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            // ROS_INFO("son1 相对于 son2的信息: 父级:%s,子级:%s 偏移量(%.2f,%.2f,%.2f)",
            //         son1ToSon2.header.frame_id.c_str(),   //turtle2
            //         son1ToSon2.child_frame_id.c_str(),    //turtle1
            //         son1ToSon2.transform.translation.x,
            //         son1ToSon2.transform.translation.y,
            //         son1ToSon2.transform.translation.z);  

            //B.根据相对计算并组织速度消息
            geometry_msgs::Twist twist;
            /*  
                组织速度，只需要设置线速度的 x 与 角速度的 z
                x = 系数 * 开方（y^2 + x^2）
                z = 系数 * 反正切（对边，邻边）
            */
            twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2));
            twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);

            //C.发布
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    // 5.spinOnce()；
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77

5.运行

使用 launch 文件组织需要运行的节点，内容示例如下:

<launch>
    <!-- 1.启动乌龟GUI节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
    
    <!-- 2.生成新的乌龟的节点 -->
    <node pkg="tf04_test" type="test01_new_turtle" name="turtle2" output="screen" />

    <!-- 3.需要启动两个乌龟相对于世界的坐标关系的发布 -->
    <!-- 
        基本实现思路：
            1.节点只编写一个
            2.这个节点需要启动两次
            3.节点启动时动态传参：第一次启动传递turtle1 第二次启动传递turtle2
     -->
    <node pkg="tf04_test" type="test02_pub_turtle" name="pub1" args="turtle1" output="screen" />
    <node pkg="tf04_test" type="test02_pub_turtle" name="pub2" args="turtle2" output="screen" />

    <!-- 4. 需要订阅 turtle1 与 turtle2 相对于世界坐标系的坐标消息，并转换成 turtle1 相对于 turtle2 的坐标关系
            再生成速度消息
    -->
    <node pkg="tf04_test" type="test03_control_turtle2" name="control" args="turtle2" output="screen" />

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

5.1.7 TF2与TF

1.TF2与TF比较_简介

• TF2已经替换了TF，TF2是TF的超集，建议学习 TF2 而非 TF
• TF2 功能包的增强了内聚性，TF 与 TF2 所依赖的功能包是不同的，TF 对应的是tf包，TF2 对应的是tf2和tf2_ros包，在 TF2 中不同类型的 API 实现做了分包处理。
• TF2 实现效率更高，比如在:TF2 的静态坐标实现、TF2 坐标变换监听器中的 Buffer 实现等

2.TF2与TF比较_静态坐标变换演示

接下来，我们通过静态坐标变换来演示TF2的实现效率。

2.1启动 TF2 与 TF 两个版本的静态坐标变换

TF2 版静态坐标变换:rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /base_link /laser

TF 版静态坐标变换:rosrun tf static_transform_publisher 0 0 0 0 0 0 /base_link /laser 100

会发现，TF 版本的启动中最后多一个参数，该参数是指定发布频率

2.2运行结果比对

使用rostopic查看话题，包含/tf与/tf_static, 前者是 TF 发布的话题，后者是 TF2 发布的话题，分别调用命令打印二者的话题消息

rostopic echo /tf: 当前会循环输出坐标系信息

rostopic echo /tf_static: 坐标系信息只有一次

2.3结论

如果是静态坐标转换，那么不同坐标系之间的相对状态是固定的，既然是固定的，那么没有必要重复发布坐标系的转换消息，很显然的，tf2 实现较之于 tf 更为高效

5.1.8 小结

坐标变换在机器人系统中是一个极其重要的组成模块，在 ROS 中 TF2 组件是专门用于实现坐标变换的，TF2 实现具体内容又主要介绍了如下几部分:

1.静态坐标变换广播器，可以编码方式或调用内置功能包来实现(建议后者)，适用于相对固定的坐标系关系

2.动态坐标变换广播器，以编码的方式广播坐标系之间的相对关系，适用于易变的坐标系关系

3.坐标变换监听器，用于监听广播器广播的坐标系消息，可以实现不同坐标系之间或同一点在不同坐标系之间的变换

4.机器人系统中的坐标系关系是较为复杂的，还可以通过 tf2_tools 工具包来生成 ros 中的坐标系关系图

5.当前 TF2 已经替换了 TF，官网建议直接学习 TF2，并且 TF 与 TF2 的使用流程与实现 API 比较类似，只要有任意一方的使用经验，另一方也可以做到触类旁通

5.2 rosbag

机器人传感器获取到的信息，有时我们可能需要时时处理，有时可能只是采集数据，事后分析，比如:

机器人导航实现中，可能需要绘制导航所需的全局地图，地图绘制实现，有两种方式，方式1：可以控制机器人运动，将机器人传感器感知到的数据时时处理，生成地图信息。方式2：同样是控制机器人运动，将机器人传感器感知到的数据留存，事后，再重新读取数据，生成地图信息。两种方式比较，显然方式2使用上更为灵活方便。

在ROS中关于数据的留存以及读取实现，提供了专门的工具: rosbag。

---

概念

是用于录制和回放 ROS 主题的一个工具集。

作用

实现了数据的复用，方便调试、测试。

本质

rosbag本质也是ros的节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息。

---

另请参考:

• http://wiki.ros.org/rosbag

5.2.1 rosbag使用_命令行

需求:

ROS 内置的乌龟案例并操作，操作过程中使用 rosbag 录制，录制结束后，实现重放

实现:

1.准备

创建目录保存录制的文件

mkdir ./xxx
cd xxx
1
2

2.开始录制

rosbag record -a -O 目标文件
1

操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件。

roscore 

rosrun turtlesim turtlesim_node 

rosrun turtlesim turtle_teleop_key 
1
2
3
4
5

3.查看文件

rosbag info 文件名
1

4.回放文件

rosbag play 文件名
1

重启乌龟节点，会发现，乌龟按照录制时的轨迹运动。

---

另请参考:

• http://wiki.ros.org/rosbag/Commandline

5.2.2 rosbag使用_编码

命令实现不够灵活，可以使用编码的方式，增强录制与回放的灵活性,本节将通过简单的读写实现演示rosbag的编码实现。

---

方案A:C++实现

1.写 bag

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "std_msgs/String.h"

/* 
    需求：使用 rosbag 向磁盘文件写出数据 （话题 + 消息）
    流程：
        1.导包
        2.初始化操作；
        3.创建rosbag对象；
        4.打开文件流；
        5.写数据；
        6.关闭文件流。

*/

int main(int argc, char *argv[])
{
    // 2.初始化操作；
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"bag_write");
    ros::NodeHandle nd;
    // 3.创建rosbag对象；
    rosbag::Bag bag;
    // 4.打开文件流；
    bag.open("hello.bag",rosbag::BagMode::Write);
    // 5.写数据；
    std_msgs::String msg;
    msg.data = "hello xxxx";
    /*  
        参数1：话题
        参数2：时间戳
        参数3：消息
    */
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    // 6.关闭文件流。
    bag.close();

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

2.读bag

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "std_msgs/String.h"

/*  
    需求：使用 rosbag 向磁盘文件写出数据 （话题 + 消息）
    流程：
        1.导包
        2.初始化操作；
        3.创建rosbag对象；
        4.打开文件流(以读的方式打开)；
        5.读数据；
        6.关闭文件流。
*/

int main(int argc, char *argv[])
{
    // 2.初始化操作；
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"bag_read");
    ros::NodeHandle nh;
    // 3.创建rosbag对象；
    rosbag::Bag bag;
    // 4.打开文件流(以读的方式打开)；
    bag.open("hello.bag",rosbag::BagMode::Read);
    // 5.读数据；消息
    // 取出话题，时间戳和消息内容
    //可以先获取消息的集合，再迭代取出消息的字段
    for (auto &&m : rosbag::View(bag))
    {
        //解析
        std::string topic = m.getTopic();
        ros::Time time = m.getTime();
        std_msgs::StringPtr p = m.instantiate<std_msgs::String>();
        ROS_INFO("解析的内容,话题:%s,时间戳:%.2f,消息值:%s",
                topic.c_str(),
                time.toSec(),
                p->data.c_str()
                );
    }
    
    // 6.关闭文件流。
    bag.close();
    
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

另请参考:

• [http://wiki.ros.org/rosbag/Code%20API](http://wiki.ros.org/rosbag/Code API)

5.3 rqt工具箱

之前，在 ROS 中使用了一些实用的工具,比如: ros_bag 用于录制与回放、tf2_tools 可以生成 TF 树 … 这些工具大大提高了开发的便利性，但是也存在一些问题: 这些工具的启动和使用过程中涉及到一些命令操作，应用起来不够方便，在ROS中，提供了rqt工具箱，在调用工具时以图形化操作代替了命令操作，应用更便利，提高了操作效率，优化了用户体验。

---

概念

ROS基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是rqt

作用

可以方便的实现 ROS 可视化调试，并且在同一窗口中打开多个部件，提高开发效率，优化用户体验。

组成

rqt 工具箱组成有三大部分

• rqt——核心实现，开发人员无需关注
• rqt_common_plugins——rqt 中常用的工具套件
• rqt_robot_plugins——运行中和机器人交互的插件(比如: rviz)

---

另请参考:

• http://wiki.ros.org/rqt

5.3.1 rqt安装启动与基本使用

1.安装

• 一般只要你安装的是desktop-full版本就会自带工具箱

• 如果需要安装可以以如下方式安装

  $ sudo apt-get install ros-noetic-rqt
  $ sudo apt-get install ros-noetic-rqt-common-plugins
  1
  2

2.启动

rqt的启动方式有两种:

• 方式1:rqt
• 方式2:rosrun rqt_gui rqt_gui

3.基本使用

启动 rqt 之后，可以通过 plugins 添加所需的插件

5.3.2 rqt常用插件:rqt_graph

**简介:**可视化显示计算图

**启动:**可以在 rqt 的 plugins 中添加，或者使用rqt_graph启动

圆框是节点，方框是话题，箭头是一个数据的发布订阅方向

5.3.3 rqt常用插件:rqt_console

**简介:**rqt_console 是 ROS 中用于显示和过滤日志的图形化插件

**准备:**编写 Node 节点输出各个级别的日志信息

5.3.3 rqt常用插件:rqt_console

**简介:**rqt_console 是 ROS 中用于显示和过滤日志的图形化插件

**准备:**编写 Node 节点输出各个级别的日志信息

/*  
    ROS 节点:输出各种级别的日志信息

*/
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"log_demo");
    ros::NodeHandle nh;

    ros::Rate r(0.3);
    while (ros::ok())
    {
        ROS_DEBUG("Debug message d");
        ROS_INFO("Info message oooooooooooooo");
        ROS_WARN("Warn message wwwww");
        ROS_ERROR("Erroe message EEEEEEEEEEEEEEEEEEEE");
        ROS_FATAL("Fatal message FFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
        r.sleep();
    }


    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

启动:

可以在 rqt 的 plugins 中添加，或者使用rqt_console启动

5.3.4 rqt常用插件:rqt_plot

**简介:**图形绘制插件，可以以 2D 绘图的方式绘制发布在 topic 上的数据

**准备:**启动 turtlesim 乌龟节点与键盘控制节点，通过 rqt_plot 获取乌龟位姿

**启动:**可以在 rqt 的 plugins 中添加，或者使用rqt_plot启动

5.3.5 rqt常用插件:rqt_bag

**简介:**录制和重放 bag 文件的图形化插件

**准备:**启动 turtlesim 乌龟节点与键盘控制节点

**启动:**可以在 rqt 的 plugins 中添加，或者使用rqt_bag启动

录制:

重放:

5.4 本章小结

本章主要介绍了ROS中的常用组件，内容如下:

• TF坐标变换(重点)
• rosbag 用于ros话题的录制与回放
• rqt工具箱，图形化方式调用组件，提高操作效率以及易用性

其中 TF坐标变换是重点，也是难点，需要大家熟练掌握坐标变换的应用场景以及代码实现。下一章开始将介绍机器人系统仿真，我们将在仿真环境下，创建机器人、控制机器人运动、搭建仿真环境，并以机器人的视角去感知世界。

参考和致谢：

https://www.bilibili.com/video/BV1Ci4y1L7ZZ?p=1&vd_source=45da538703ca980e21d6c2bff2a7c144

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

万分感谢赵老师事无巨细的视频教学和清晰明了的笔记～