五、ROS常用组件
在ROS中内置了一些比较使用的工具；本章主要介绍ROS内置如下组件：
TF坐标变换，实现不同类型坐标系之间的转换；
rosbag录制ROS节点执行过程，实现回放该过程的效果；
rqt工具箱，图形化调试工具；
1.TF坐标变换
机器人系统有多个传感器用于感知周围环境信息，但是直接将物体相对于传感器的坐标信息与机器人系统与物体之间的坐标方位信息划等号是错误的，需要经过转化过程。
不同坐标之间的转化相对复杂繁琐，ROS之中直接封装了相关的模块：坐标变换(TF);
**概念：**tf：坐标变换；
**坐标系：**ROS是通过坐标系统对物体进行标定，通过右手坐标系来标定；
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-YOtVV6YC-1668337713252)(C:\Users\昊天\AppData\Roaming\Typora\typora-user-images\1668314087211.png)]
**作用：**在ROS之中用于实现不同坐标系之间的点或者向量的转化；
**说明：**tf被弃用，现在使用的是tf2，它耿简洁高效，相对应的功能包有：
tf2_geometry_msgs:ROS消息转化为TF2消息；
tf2：封装坐标变化常用消息；
tf2_ros: 为tf2提供roscpp和rospy绑定，封装了坐标变换常用API；
1.1 坐标msg消息
在坐标变换中常用的数据传输载体msg是：
geometry_msgs/TransformStamped
geometry_msgs/PointStamped
前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息；
- 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
float64 y
float64 z
geometry_msgs/Quaternion rotation #四元数
float64 x
float64 y
float64 z
float64 w
解释：
- A与B两个坐标系，讨论A坐标系相对于B坐标系的关系，此时A是child_frame_id，而B是frame_id.
- 四元数是另一种形式的欧拉角(翻转，俯仰，旋转)，但是欧拉角存在奇异性，写入代码会出现bug，因此选择形如复数的四元数代替。
- geometry_msgs/PointStamped
终端输入：
rosmsg info geometry_msgs/PointStamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Point point
float64 x
float64 y
float64 z
1.2 静态坐标变换
静态坐标变换，之两个坐标系之间的相对位置是固定的。
基本实现逻辑：
- 坐标系相对关系，通过发布方发送
- 订阅方根据订阅发布方的坐标系相关关系，传入坐标点信息(可以写死)，借助tf实现坐标变换，并将结果输出。
基本实现流程：
- 功能包，添加tf依赖
- 编写发布方
- 编写订阅方
方案A：C++
实验：
1.创建新功能包demo04_ws,在其src文件下创建新的.cpp文件应该包含以下几个依赖包：
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs
2.配置新code快捷编译tasks.json文件：
{
// 有关 tasks.json 格式的文档，请参见
// https://go.microsoft.com/fwlink/?LinkId=733558
"version": "2.0.0",
"tasks": [
{
"label": "catkin_make:debug", //代表提示的描述性信息
"type": "shell", //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
"command": "catkin_make",//这个是我们需要运行的命令
"args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
"group": {
"kind":"build","isDefault":true},
"presentation": {
"reveal": "always"//可选always或者silence，代表是否输出信息
},
"problemMatcher": "$msCompile"
}
]
}
- 发布方
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TwistWithCovarianceStamped.h"
#include "tf2/LinearMath/Quaternion.h"//欧拉角
/**
* @brief 需求：该节点需要发布两个坐标系之间的关系
*
* 流程：
* 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); //目前研究对象雷达相对于小车是固定的，所以没有翻滚，俯仰，偏航 等因此设为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;
}
rostopic list
/rosout
/rosout_agg
/tf_static
rostopic echo /tf_static
transforms:
-
header:
seq: 0
stamp:
secs: 1668322909
nsecs: 686407571
frame_id: "base_link"
child_frame_id: "laser"
transform:
translation:
x: 0.2
y: 0.0
z: 0.5
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
使用rviz显示：
rviz
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dOI1x1PP-1668337713255)(C:\Users\昊天\AppData\Roaming\Typora\typora-user-images\1668324095841.png)]
- 订阅方
#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"
/**
* @brief 订阅方
* 需求：
*订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换
头发》订阅坐标系相对关系
* 4、组织一个坐标点的数据
* 5、转换算法实现 调用tf内置实现
* 6、输出转换结果
*/
int main(int argc, char* argv[])
{
// * 2、编码 初始化 nodehandle 必须有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：目标坐标系
返回值：输出的坐标点
注意：调用是必须包含头文件 tf2_geometry_msgs/
注意：运行时候出现报错，提示base_link不存在
原因分析：是因为发布方可能还没有发布 ，订阅数据是一个耗时操作，可能调用转换时，坐标系的相对关系还没有订阅
解决办法：方案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());
}
// 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;
}
执行结果：
[ INFO] [1668329057.735620630]: 异常消息:"base_link" passed to lookupTransform argument target_frame does not exist.
[ INFO] [1668329057.835439384]: 转换后的坐标值:(2.20,3.00,5.50),参考的坐标系:base_link
补充：
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、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 /base_link /camera
rviz
rosrun tf2_ros static_transform_publisher 0.1 0.0 0.3 1.0 0 0 /base_link /camera
在rviz中可以看见转换效果，也建议使用该种方式直接实现静态坐标系相对信息发布。