ROS入门跟着我就够了（五） ROS常用组件
在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件:
TF坐标变换，实现不同类型的坐标系之间的转换；
rosbag 用于录制ROS节点的执行过程并可以重放该过程；
rqt 工具箱，集成了多款图形化的调试工具。
本章预期达成的学习目标:
了解 TF 坐标变换的概念以及应用场景；
能够独立完成TF案例:小乌龟跟随；
可以使用 rosbag 命令或编码的形式实现录制与回放；
能够熟练使用rqt中的图形化工具。
案例演示: 小乌龟跟随实现，该案例是ros中内置案例，终端下键入启动命令
roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch //c++实现
或者
roslaunch turtle_tf2 turtle_tf2_demo.launch //python实现
键盘可以控制一只乌龟运动，另一只跟随运动。
实现如下：
5.1 TF坐标变换
当然，在明确了不同坐标系之间的的相对关系，就可以实现任何坐标点在不同坐标系之间的转换，但是该计算实现是较为常用的，且算法也有点复杂，因此在 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。（坐标变换的发布和订阅对象）
5.1.1 坐标msg消息
订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped
前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。
geometry_msgs/PointStamped
1 坐标点消息
seq 序列化号
time stamp 时间戳(表示以哪个坐标点为参考物的)
frame_id 坐标点以哪个坐标系为参考物的
point 组成 （具体坐标值）
2 两个坐标系的关系
seq 序列化号
time stamp 时间戳(表示以哪个坐标点为参考物的)
frame_id 被参考的坐标系
child_frame_id 另一个坐标系
point 组成 （具体坐标值）
translation 是frame id 到child的偏移量
geometry_msgs/Vector3 translation #偏移量
float64 x #|-- X 方向的偏移量
float64 y #|-- Y 方向的偏移量
float64 z #|-- Z 方向上的偏移量
rotation 是fram id 到child翻滚俯仰偏航角度的变化（四元数表示）
float64 x
float64 y
float64 z
float64 w
5.1.2 静态坐标变换
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
需求描述:
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
实现分析:
坐标系相对关系，可以通过发布方发布
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出
实现流程:C++ 与 Python 实现流程一致
新建功能包，添加依赖
编写发布方实现
编写订阅方实现
执行并查看结果
C++实现
发布方：
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
导入包，还挺多的 ，看图
2 配置Cmake, 编译
3 编写发布方程序
直接写main构建框架
运用rosmsg 查看 消息格式
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
// 需求：发布两个坐标系的相对关系
// 流程： 1.包含头文件（一般先不写，用到谁写谁）
// 2 节点初始化
// 3 创建发布对象，创建静态坐标转换广播器
// 4 组织被发布的消息
// 5 发布数据，广播器发布坐标系信息
// （发布逻辑不用，因为是静态的，所以只需要发布一次就够了）
// 6 spin()
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2 节点初始化
ros::init(argc,argv,"static_pub");//节点名字
ros::NodeHandle nh;
// 3 创建发布对象
tf2_ros::StaticTransformBroadcaster pub;//创建静态坐标转换广播器对象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;
//根据欧拉角转换为四元数
//包含头文件quaternion
tf2::Quaternion qtn;//创建四元数对象
//向该对象设置欧拉角，设置完之后，这个对象可以将欧拉角转换成四元数
qtn.setRPY(0,0,0);//如果是按倒过来了，那要设置成3.14,欧拉角单位是弧度
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;
}
通过rostopic echo完成
通过rviz可视化查看
红色x轴
绿色y轴
蓝色z轴
订阅方实现
再看一遍rosmsg info
订阅代码如下：
#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[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"static_sub");
ros::NodeHandle nh;
// 3.创建订阅对象：订阅坐标系相对关系(添加头文件*2，配套使用) 需要创建buffer缓存，以及监听对象
//监听对象可以将订阅的数据存入buffer中
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 4.组织一个坐标点数据(添加头文件)#include"geometry_msgs/PointStamped.h"
geometry_msgs::PointStamped ps;
ps.header.frame_id="laser";
ps.header.stamp=ros::Time::now();
ps.point.x=2.0;
ps.point.y=5.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_out= buffer.transform(ps,"base_link");//调用时需要包含头文件#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
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;
}
报错：
已经开始准备转换了，发布的相对关系还没有，所以报错
terminate called after throwing an instance of 'tf2::LookupException'
what(): "base_link" passed to lookupTransform argument target_frame does not exist.
已放弃 (核心已转储)
方法1：
那么这里是要在转换之前添加延时函数(如上面代码所示)
ros::Duration(2).sleep();
方法2：进行异常处理，捕获异常，直到没有了程序继续执行
#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[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"static_sub");
ros::NodeHandle nh;
// 3.创建订阅对象：订阅坐标系相对关系(添加头文件*2，配套使用) 需要创建buffer缓存，以及监听对象
//监听对象可以将订阅的数据存入buffer中
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 4.组织一个坐标点数据(添加头文件)#include"geometry_msgs/PointStamped.h"
geometry_msgs::PointStamped ps;
ps.header.frame_id="laser";
ps.header.stamp=ros::Time::now();
ps.point.x=2.0;
ps.point.y=5.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;
try
{
ps_out= buffer.transform(ps,"base_link");//需要包含头文件#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
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)
{
ROS_INFO("异常消息：%s",e.what());
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
Python实现
订阅方
#! /usr/bin/env python
#coding=utf-8
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
# 发布坐标系相对关系（小车底盘 baselink 雷达 laser）
# 1 导包
# 2 初始化ros节点
# 3 创建发布对象
# 4 组织被发布的数据
# 5 发布数据
# 6 spin()
if __name__=="__main__":
# 2 初始化ros节点
rospy.init_node("static_pub_p")
# 3 创建发布对象(导包import tf2_ros)
pub=tf2_ros.StaticTransformBroadcaster()
# 4 组织被发布的数据（导包from geometry_msgs.msg import TransformStamped )
ts=TransformStamped()
#header
ts.header.stamp=rospy.Time.now()
ts.header.frame_id="base_link"
#child frame
ts.child_frame_id="laser"
#相对关系（偏移 四元数）
ts.transform.translation.x=0.2
ts.transform.translation.y=0.0
ts.transform.translation.z=0.5
#从欧拉角转换到四元数(导入包import tf),返回一个数组
qtn=tf.listener.transformations.quaternion_from_euler(0,0,0)
#设置四元数
ts.transform.rotation.x=qtn[0]
ts.transform.rotation.y=qtn[1]
ts.transform.rotation.z=qtn[2]
ts.transform.rotation.w=qtn[3]
# 5 发布数据
pub.sendTransform(ts)
# 6 spin()
rospy.spin()
查看方法1：rostopic echo
查看方法2：rviz
选择底盘以及TF
订阅方：
#! /usr/bin/env python
#coding=utf-8
from ast import Expression, Try
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
# 订阅方：订阅坐标转换信息，传入被转换的坐标点，调用转换算法
# 流程：
# 1 导包
# 2 初始化ros节点
# 3 创建订阅对象
# 4 组织被转换的坐标点
# 5 转换逻辑实现，调用tf封装的算法
# 6 输出结果
if __name__=="__main__":
# 2 初始化ros节点
rospy.init_node("static_sub_p")
# 3 创建订阅对象
#3.1 创建缓存对象
buffer=tf2_ros.Buffer()
#3.2创建订阅对象（将缓存传入）import tf2_ros
tf2_ros.TransformListener(buffer)
# 4 组织被转换的坐标点 from tf2_geometry_msgs import tf2_geometry_msgs
ps=tf2_geometry_msgs.PointStamped()
ps.header.frame_id=rospy.Time.now()
ps.header.frame_id="laser"
ps.point.x=2.0
ps.point.y=3.0
ps.point.z=5.0
# 5 转换逻辑实现，调用tf封装的算法
rate=rospy.Rate(10)
while not rospy.is_shutdown():
try:
#转换实现
ps_out= buffer.transform(ps,"base_link")
# 6 输出结果
rospy.loginfo("转换后的坐标：（%.2f,%.2f,%.2f),参考的坐标系：%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id)
except expression as e:
rospy.logwarn("错误%s",e)
rate.sleep()
补充知识点：
1.rviz
这个介绍过了不再介绍了
2
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
也建议使用该种方式直接实现静态坐标系相对信息发布。
