ROS学习笔记06、ROS常用组件（TF坐标变换、rosbag、rqt工具箱）

前言

马上开学，目前学校很多实验室都是人工智能这块，大部分都是和机器人相关，然后软件这块就是和cv、ros相关，就打算开始学习一下。

本章节是虚拟机安装Ubuntu18.04以及安装ROS的环境。

学习教程：【Autolabor初级教程】ROS机器人入门，博客中一些知识点是来源于赵老师的笔记在线笔记，本博客主要是做归纳总结，如有侵权请联系删除。

视频中的案例都基本敲了遍，这里给出我自己的源代码文件：

链接：https://pan.baidu.com/s/13CAzXk0vAWuBsc4oABC-_g 
提取码：0hws 

所有博客文件目录索引：博客目录索引(持续更新)

一、TF坐标变换

1.1、坐标msg消息

坐标转换实现中常用的 msg：geometry_msgs/TransformStamped和geometry_msgs/PointStamped。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

• 前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。

查询geometry_msgs/TransformStamped消息：

# 查询geometry_msgs/TransformStamped 
rosmsg info geometry_msgs/TransformStamped 

# 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

查询geometry_msgs/PointStamped消息：

# 查询geometry_msgs/PointStamped
rosmsg info geometry_msgs/PointStamped

# 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、静态坐标变换

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

实现方案：发布geometry_msgs/TransformStamped格式的消息，接着使用订阅者来接收该坐标系相关位置信息，在订阅者端使用转换算法以及对应的障碍物坐标来得到base_link的坐标点。

• 对于其中发布者替代方案我们不需要自己去手动编写发布者代码，可直接使用ros给我们提供的tf2_ros static_transform_publisher来进行实现。

发布者实现

模拟实际应用，实际场景中会从雷达里传出来。

# 进入到工程下的src目录
cd /home/workspace/roslearn/src

# 创建名为06tf_static的包
catkin_create_pkg --rosdistro melodic 06tf_static roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

demo01_static_pub.cpp：

#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::init(argc, argv, "static_brocast");
    //3、创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    //4、创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //---设置头信息
    ts.header.seq = 1;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    //--设置子级坐标系
    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();
    //5、广播发布坐标系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;
}

修改CMakeLists.txt：

add_executable(demo01_static_pub src/demo01_static_pub.cpp)

add_dependencies(demo01_static_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo01_static_pub
  ${catkin_LIBRARIES}
)

接着进行编译与运行发布者：

source ./devel/setup.bash

rosrun 06tf_static demo01_static_pub

接着我们可以去查看下当前的话题信息并进行打印：

rostopic list

rostopic echo /tf_static

rviz可视化工具

在启动发布节点之后，我们就可以运行rviz工具，来进行可视化查看：

rviz

Add添加的是TF：

替代发布者实现（ros工具包）

示例针对于上面发布者实现，我们这里可以调用ros提供的工具包来进行调用：

# 运行工具包
rosrun tf2_ros static_transform_publisher 1.5 0 0.5 0 0 0 /base_link /laser

---

订阅者实现

demo01_static_sub.cpp：

#include "ros/ros.h"
//对应tf2_ros::TransformListener监听器
#include "tf2_ros/transform_listener.h"
//对应的是tf2_ros::Buffer节点（用于监听器来接收数据的一块缓冲区）
#include "tf2_ros/buffer.h"
//对应的是地图消息的坐标标识
#include "geometry_msgs/PointStamped.h"
//若是不引入该依赖会报错
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"


int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    //2、初始化ROS节点
    ros::init(argc, argv, "tf_sub");
    ros::NodeHandle nh;
    //3、创建 TF 订阅节点
    tf2_ros::Buffer 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;
        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()
                    );
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

编辑修改CMakeLists.txt：

add_executable(demo02_static_sub src/demo02_static_sub.cpp)

add_dependencies(demo02_static_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo02_static_sub
  ${catkin_LIBRARIES}
)

编译整个项目，并进行运行节点：

source ./devel/setup.bash

# 启动订阅者
rosrun 06tf_static demo01_static_sub

tf2::LookupException报错解决方案

报错：tf2::LookupException

原因描述：tf2_ros::TransformListener listener(buffer);还没有接收到消息的时候就往下执行到了transform转换方法，此时就会报错。

• 如何解决呢？就是说要等到接收到消息了才往下执行就不会异常结束了！

解决方案一：在接收消息下添加一个延时函数。

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

解决方案二：在对应执行转换算法的时候我们去进行一个try catch捕捉异常即可。

//核心代码：将ps转换为base_link的坐标点
geometry_msgs::PointStamped ps_out;
try{   
    //转换算法
    ps_out = buffer.transform(ps, "base_link");
}catch(const std::exception& e){
    std::cerr << e.what() << '\n';
}

1.3、动态坐标变换

实现需求：

• 发布方：订阅乌龟的坐标话题，乌龟坐标转为坐标系相对信息，接着将对应的坐标系相对信息通过广播发布出去。
• 订阅方：订阅到坐标系相对信息，接着对其进行转换得到该乌龟相对于主体的坐标。

发布者

流程：自己编写的发布者去订阅ros乌龟的话题，其中会将乌龟的坐标消息转为TransformStamped消息类型然后通过广播器发布出去（/tf），接着可以使用rviz图形化工具来去订阅（world、tf的坐标信息，来实现一个乌龟的3D坐标同步）。

demo02_dynamic_pub.cpp：动态坐标系的发布者

#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) {
    ROS_INFO("获取乌龟的位姿：x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f", pose->x, pose->y, pose->theta, pose->linear_velocity, pose->angular_velocity);
    //获取位姿信息，转换成坐标相对关系（核心），并发布
    //a、创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //b、组织被发布的数据
    geometry_msgs::TransformStamped tfs;
    // |-- 头设置
    tfs.header.frame_id = "world"; //坐标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;
    //坐标轴四元数
    /*
        位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是2D，没有翻滚和俯仰角度，所以
        可以得到乌龟的欧拉角：0 0 theta
     */
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //c、广播器发布数据
    pub.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;
    //1、创建订阅对象，订阅 /turtle1/pose
    ros::Subscriber sub = nh.subscribe("/turtle1/pose", 100, doPose);
    //2、回调函数处理订阅的消息：将坐姿信息转化为坐标相对关系并发布（关注）
    //3、spin()回调函数
    ros::spin();
    return 0;
}

修改CMakeLists.txt文件：

add_executable(demo02_dynamic_pub src/demo02_dynamic_pub.cpp)

add_dependencies(demo02_dynamic_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo02_dynamic_pub
  ${catkin_LIBRARIES}
)

最后来进行实操启动下：

1、启动ros核心以及GUI及键盘控制

<launch>
    
    <node pkg="turtlesim" type="turtlesim_node"  name="myTurtle" output="screen" />
    
    <node pkg="turtlesim" type="turtle_teleop_key"  name="myTurtleContro" output="screen" />
launch>

# 启动GUI以及键盘控制
roslaunch 06tf_static 01start_turtle.launch

2、启动动态坐标变换的发布者

source ./devel/setup.bash

# 启动发布者
rosrun 06tf_static demo02_dynamic_pub

3、查看坐标话题

rostopic list

4、打开图形化界面rviz工具，选择fixed Frame【world】，Add【TF】，然后去操控键盘去实现动态的信息发布

# 选择Fixed Frame为world，add TF坐标
rviz

---

订阅者

//修改1：表示当前的frame_id为turtle1
ps.header.frame_id = "turtle1";
//修改2：时间戳为0.0
ps.header.stamp = ros::Time(0.0);、
    
//修改3：与发布方的坐标id一致都是world
ps_out = buffer.transform(ps, "world");

对于时间戳更改为0.0的解释：

• 动态坐标变换中buffer是有许多值，而之前静态坐标变换就只有一个值，其中每个动态发布中也有时间戳并且是动态的，在订阅者监听之后肯定是有延时的，转化的时候会拿你当前设置的时间戳与之前发布消息的时间戳进行比对，比较判断两个时间戳是否是接近的，若是接近就可以转，若是时间差距比较大就不会允许。
• 若是直接设置为0.0，那么在底层的代码发现你没有设置就不会进行比对了，也就不会抛出异常了。

按照上方的发布者发布好之后，我们来去运行订阅者：

source ./devel/setup.bash

# 启动发布者
rosrun 06tf_static demo02_dynamic_sub

---

1.4、多坐标变换

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

发布者

直接使用ros提供的工具包来进行发布两个相对于世界的坐标：

<launch>
    <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>

来进行启动：

# 运行launch文件
roslaunch 06tf_static 03_tfs.launch

# 打开GUI坐标系
rviz

订阅者

demo03_tfs.cpp：

#include "ros/ros.h"
//引入监听器：tf2_ros::TransformListener
#include "tf2_ros/transform_listener.h"
//缓冲区
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
//若是不引入该依赖会报错
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

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

    //创建坐标点
    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：目标坐标系 B
                参数2：源坐标系   A
                参数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
                ); //son1

            //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();
    }
    return 0;
}

修改CMakeLists.txt：

add_executable(demo03_tfs src/demo03_tfs.cpp)

add_dependencies(demo03_tfs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo03_tfs
  ${catkin_LIBRARIES}
)

开始进行编译运行：

source ./devel/setup.bash

# 启动发布者☀️
rosrun 06tf_static demo03_tfs

---

1.5、坐标关系查看（tf2_tools）

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

步骤一：安装tf2_tools

# 注意命令格式：ros-【ros版本】-tf2-tools
sudo apt-get install ros-melodic-tf2-tools

步骤二：发布坐标关系

03_tfs.launch：

<launch>
    <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>

执行发布命令：

# 运行launch文件
roslaunch 06tf_static 03_tfs.launch

步骤三：启动坐标系广播程序之后，来生成相应的pdf文档

# 首先启动发布两个坐标点，接着就可以在当前目录下生成pdf文档
rosrun tf2_tools view_frames.py

# 打开pdf
evince frames.pdf

---

TF坐标变换实操

实际效果演示即方案

实际效果演示：

实现介绍说明：

1、发布新建乌龟服务。

• service：/spawn。
• 服务srv：turtlesim::Spawn。

2、订阅两个乌龟的pose坐标，转换为世界坐标来进行广播发布。

• 订阅topic：/turtle1/pose、/turtle2/pose；msg：turtlesim::Pose
• 发布广播(topic)：tf2_ros::TransformBroadcaster；msg：geometry_msgs::TransformStamped。

3、订阅广播，获取到turtleA、turtleB并计算其相对的距离，并使用数学公式来计算出速度，发布turtleB的坐标即可实现乌龟2的跟随。

• 订阅广播(topic)：geometry_msgs::TransformStamped。
• 发布topic：/turtle2/cmd_vel；msg：geometry_msgs::Twist。

创建新功能包：

# 进入到工程下的src目录
cd /home/workspace/roslearn/src

# 创建名为06tf_practical的包
catkin_create_pkg --rosdistro melodic 06tf_practical roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

---

功能1：新建乌龟

01_new_turtle.cpp：

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

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    //1、初始化节点
    ros::init(argc,  argv, "setTurtle_node");

    //2、实例化句柄
    ros::NodeHandle nh;

    //3、使用句柄实例化服务客户端
    ros::ServiceClient client = nh.serviceClient("/spawn");

    //4、等待服务启动
    //方式一：使用客户端实例的方法
    // client.waitForExistence();
    //方式二：使用ros提供的方法
    ros::service::waitForService("/spawn");

    //5、发送请求
    //填充request请求对象
    turtlesim::Spawn spawn;
    spawn.request.x = 1;
    spawn.request.y = 1;
    spawn.request.theta = 1.5;
    spawn.request.name = "turtle2";

    bool flag = client.call(spawn);
    if (flag) {
        ROS_INFO("生成乌龟成功！乌龟的名称为：%s", spawn.response.name.c_str());
    }else {
        ROS_INFO("生成乌龟失败！");
    }

    return 0;
}

功能2：订阅乌龟坐标并广播发布

02_turtle_pub.cpp：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
// 广播器
#include "tf2_ros/transform_broadcaster.h"
// 转换标记坐标
#include "geometry_msgs/TransformStamped.h"
// 欧拉角引入
#include "tf2/LinearMath/Quaternion.h"

//乌龟名称
std::string turtle_name;

//处理坐标转换并发布
void doPose(const turtlesim::Pose::ConstPtr& pose) {
    // ROS_INFO("获取乌龟的位姿：x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f", pose->x, pose->y, pose->theta, pose->linear_velocity, pose->angular_velocity);
    //获取位姿信息，转换成坐标相对关系（核心），并发布
    //a、创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //b、组织被发布的数据
    geometry_msgs::TransformStamped tfs;
    // |-- 头设置
    tfs.header.frame_id = "world"; //坐标id表示为world
    tfs.header.stamp = ros::Time::now();
    // |-- 坐标系ID
    tfs.child_frame_id = turtle_name;
    // |-- 坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0;
    //坐标轴四元数
    /*
        位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是2D，没有翻滚和俯仰角度，所以
        可以得到乌龟的欧拉角：0 0 theta
     */
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //c、广播器发布数据
    pub.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "dynamic_pub");

    //校验参数
    if (argc != 2) {
        ROS_ERROR("请传入正确的参数！");
        return 1;
    }else {
        turtle_name = argv[1];
        ROS_INFO("乌龟【%s】的坐标发送启动", turtle_name.c_str());  
    }

    ros::NodeHandle nh;
    //1、创建订阅对象，订阅 /turtle1/pose
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose", 1000, doPose);
    //2、回调函数处理订阅的消息：将坐姿信息转化为坐标相对关系并发布（关注）
    //3、spin()回调函数
    ros::spin();
    return 0;
}

---

功能3：订阅广播并计算turtle2的速度

速度计算的图示：

03_turtle2_sub.cpp：

#include "ros/ros.h"
//引入监听器：tf2_ros::TransformListener
#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/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "tfs_sub");
    ros::NodeHandle nh;
    //1、创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    //2、获取到发布对象
    ros::Publisher pub = nh.advertise("/turtle2/cmd_vel", 1000);

    ros::Rate rate(10);
    while (ros::ok()) {
        try
        {
            //1、计算turtle1和turtle2的相对关系
            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
            //     ); //son1

            //2、根据相对计算并组织速度消息
            geometry_msgs::Twist twist;
            /*
                组织速度，只需要设置线速度的x与角速度的z
                x = 系数 * 开方(y^2 + x^2)
                z = 系数 * 反正切(对边，临边)
             */
            twist.linear.x = 1 * 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);
            //发布消息
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示：%s", e.what());
        }
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

---

编写launch文件统一启动

start_turtle.launch：

<launch>
    
    <node pkg="turtlesim" type="turtlesim_node"  name="turtle1" output="screen" />
    
    <node pkg="turtlesim" type="turtle_teleop_key"  name="myTurtleContro" output="screen" />
    
    <node pkg="06tf_practical" type="01_new_turtle" name="turtle_spawn" output="screen" />
    
    <node pkg="06tf_practical" type="02_turtle_pub" name="tf_pub1" output="screen" args="turtle1" />
    <node pkg="06tf_practical" type="02_turtle_pub" name="tf_pub2" output="screen" args="turtle2" />
    
    <node pkg="06tf_practical" type="03_turtle2_sub" name="tf_sub" output="screen"  />
launch>

启动命令：

source ./devel/setup.bash

launch 06tf_practical start_turtle.launch

---

二、rosbag（数据留存与读取）

2.1、认识rosbag

ros-wiki-rosbag

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

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

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

场景：机器人导航实现中，可能需要绘制导航所需的全局地图，地图绘制实现，有两种方式：

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

创建工程：

# 进入到工程下的src目录
cd /home/workspace/roslearn/src

# 创建名为06tf_rosbag的包
catkin_create_pkg --rosdistro melodic 06tf_rosbag roscpp rospy std_msgs rosbag

---

2.2、rosbag使用命令行

熟悉命令

#  对所有话题进行录制
rosbag record -a -o bags/hello.bag

# 指定话题录制
rosbag record /turtle1/cmd_vel -o bags/hello.bag

# 查看bag信息
rosbag info bags/xxx.bag

# 回放bag信息
rosbag play bags/xxx.bag

# 查看rosbag record的命令行提示信息
rosbag record --help

实际案例

案例目标：录制一段使用turtle在GUI中移动的话题消息，并进行回放！

首先启动节点：turtle的GUI界面以及keyboard控制

roslaunch 06tf_rosbag start_turtle.launch

录制功能：打开之后我们来使用rosbag进行监听命令：

# 这里我们执行
rosbag record /turtle1/cmd_vel -o bags/hello.bag

回车即可录制结束，此时就会在当前目录下的bags中生成一个日志文件：

回放功能：

# 回放指定的bag文件
rosbag play bags/hello_2022-09-13-20-13-14.bag

---

2.3、rosbag编码实现读和写（C++）

需求：使用rosbag来向/chatter写入多条消息进行本地化存储；之后来进行读取所有的数据。

写

demo01_rosbag_write.cpp：

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

int main(int argc, char *argv[])
{
    //初始化
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "bag_write");
    ros::NodeHandle nh;

    //创建rosbag对象
    rosbag::Bag bag;

    //打开文件流
    bag.open("bags/hello.bag", rosbag::bagmode::Write);

    //写数据（像话题/chatter写消息）
    std_msgs::String msg;
    msg.data = "hello changlu";
    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);

    //关闭文件流
    bag.close();
    return 0;
}

修改CMakelists.txt：

add_executable(demo01_rosbag_write src/demo01_rosbag_write.cpp)

add_dependencies(demo01_rosbag_write ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo01_rosbag_write
  ${catkin_LIBRARIES}
)

最终进行编译进行写入bag文件：

source ./devel/setup.bash

rosrun 06tf_rosbag demo01_rosbag_write

读

demo02_rosbag_read.cpp：

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

int main(int argc, char *argv[])
{
    //初始化
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "bag_read");
    ros::NodeHandle nh;

    //创建rosbag对象
    rosbag::Bag bag;

    //打开文件流
    bag.open("bags/hello.bag", rosbag::bagmode::Read);

    //读数据
    //取出话题，时间戳和消息内容
    //可以先获取消息的集合，再迭代取出消息的字段
    for (auto &&m: rosbag::View(bag)) {
        //获取话题
        std::string topic = m.getTopic();
        //获取中取出时间戳、实体数据
        ros::Time time = m.getTime();
        std_msgs::StringPtr p = m.instantiate();
        ROS_INFO("解析得到的内容，话题：%s, 时间戳：%.2f, 消息值：%s",
              topic.c_str(),
              time.toSec(),
              p->data.c_str()
            );
    }

    //关闭文件流
    bag.close();
    return 0;
}

编辑CMakeLists.txt：

add_executable(demo02_rosbag_read src/demo02_rosbag_read.cpp)

add_dependencies(demo02_rosbag_read ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo02_rosbag_read
  ${catkin_LIBRARIES}
)

最后进行编译运行：

source ./devel/setup.bash

rosrun 06tf_rosbag demo02_rosbag_read

三、rqt工具箱

3.1、rqt工具安装与启动

安装工具如下：

# melodic、noetic
sudo apt-get install ros-melodic-rqt

sudo apt-get install ros-melodic-rqt-common-plugins

rqt的启动方式有两种:

# 方式一
rqt

# 方式二
rosrun rqt_gui rqt_gui

3.2、常用插件

其他还有：rqt_service、rqt_topic。

Node Graph（计算图）

或者直接执行命令：

rqt_graph

示例图：

---

rqt_console（日志窗口）

通过rqt工具箱打开或者直接命令行打开即可：

rqt_console

编写一段日志打印代码：

demo02_rqtconsole.cpp：

#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;
}

编译并启动节点：

source ./devel/setup.bash

rosrun 06tf_rosbag demo02_rqtconsole

接着我们去看rqt_console窗口就可以看到当前结点报的日志错误信息了：

---

rqt_plot（2D绘制topic数据）

介绍：图形绘制插件，可以以 2D 绘图的方式绘制发布在 topic 上的数据。

启动方式，GUI或者命令行：

 rqt_plot

启动乌龟GUI结点以及键盘控制节点，接着在qrt_plot中订阅位姿节点，接着进行操控：

---

rqt_rosbag

rosbag演示

启动：

rqt_bag

参考资料

[1]. rosbag录制话题、播放话题多种模式