文章目录
五、ROS常用组件
在ROS中内置了一些比较使用的工具；本章主要介绍ROS内置如下组件：
TF坐标变换，实现不同类型坐标系之间的转换；
rosbag录制ROS节点执行过程，实现回放该过程的效果；
rqt工具箱，图形化调试工具；
1.TF坐标变换
机器人系统有多个传感器用于感知周围环境信息，但是直接将物体相对于传感器的坐标信息与机器人系统与物体之间的坐标方位信息划等号是错误的，需要经过转化过程。
不同坐标之间的转化相对复杂繁琐，ROS之中直接封装了相关的模块：坐标变换(TF);
**概念：**tf：坐标变换；
**坐标系：**ROS是通过坐标系统对物体进行标定，通过右手坐标系来标定；
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-v0jRw5FA-1668434092634)(C:\Users\昊天\AppData\Roaming\Typora\typora-user-images\1668314087211.png)]
扫描二维码关注公众号，回复：
16186078 查看本文章
**作用：**在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-YxcJHdjc-1668434092636)(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中可以看见转换效果，也建议使用该种方式直接实现静态坐标系相对信息发布。
1.3 动态坐标变换
需要上一节的坐标变换发布
以及动态坐标订阅
- 发布方实现
#include <ros/ros.h>
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方：需要订阅乌龟的位资信息，转换成想对于窗体的一个坐标关系，并发布
准备：
话题：/turtle1/pose
消息：turtlesim/Pose
liucheng流程：
1、包含头文件
2、设置编码、初始化、nodehandle
3、创建订阅对象、订阅/turtle1/pose
4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
5、spin()
*/
void doPose(const turtlesim::Pose::ConstPtr& pose)//const 防止函数修改引用内容 传引用提高效率
{
//获取位资信息，转换成坐标系想对关系（核心）并发布
//创建TF发布对象
static tf2_ros::TransformBroadcaster pub; // static修饰 使得每次回调函数是哟你pub这个函数;锁定pub
//组织被发布的数据
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;
//四元数
//位资信息之中 没有四元数 有个z轴的偏航角度 而乌龟是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();
//发布
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;
}
效果展示：
- 订阅方实现
#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 = "turtle1";
//时间戳
ps.header.stamp = ros::Time(0.0);//动态坐标转换的时候buffer生成会有时间戳，时间戳不对代表坐标变化，因此报错
// ps.header.stamp = ros::Time::now();
ps.point.x = 2.0;
ps.point.y = 3.0;
ps.point.z = 0.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, "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());
}
// 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;
}
- 效果展示
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qymlplVl-1668434092640)(C:\Users\昊天\AppData\Roaming\Typora\typora-user-images\1668426497090.png)]
总结：订阅方与上一节静态订阅方实现大致流程相同，仅需要更改对应的坐标名称以及注意buffer的时间戳并不是当前的now，而应该改为0.0；
1.4 多坐标变换
**需求：**多坐标之间的变换，上两节都是两个坐标之间相对位置的转换。本节将介绍多坐标之间的变换；父级坐标world，2个子级坐标son1，son2，son1相对于world以及son2相对于world关系已知，求son1原点在son2之中的坐标，又或者已知son1之中一点的坐标，求其参考坐标son2的坐标
分析：
1.发布son1相对于world 以及son2相对于world
2.订阅发布信息，借助tf2转换son1与son2坐标关系
3.实现坐标点之间的转换
流程：
1、新建功能包，添加相关依赖
2、创建坐标相对关系发布方
3、创建坐标相对关系订阅方
- 发布方采用静态坐标变换 launch文件
<?xml version="1.0"?>
<launch>
<!-- 发布son1 相对于world 以及 son2相对于world 的坐标关系 -->
<node name="son1" pkg="tf2_ros" type="static_transform_publisher" args="5 0 0 0 0 0 /world /son1" output="screen"/>
<node name="son2" pkg="tf2_ros" type="static_transform_publisher" args="3 0 0 0 0 0 /world /son2" output="screen"/>
</launch>
- 订阅方
#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"
/**
* @brief
* 订阅方实现：1.计算son1与2之间的相对关系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：目标坐标系 B
参数2：源坐标系 A
参数3：ros::Time(0.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)
{
// std::cerr << e.what() << '\n';
}
// * 5、spinOnce
rate.sleep();
ros::spinOnce();
}
return 0;
}
又一次回顾了launch文件下的node标签里的args传参，以及生成一个坐标点以及新的函数buffer.lookupTraansform(“”,“”,ros::Time(0.0))
1.5 坐标系关系查看
- 查看是否安装过tf2_tools:’ rospack find tf2_tools ’
- 安装命令：‘ sudo apt install ros-noetic-tf2-tools ’
- roslaunch tf03_tfs tfs_c.launch
- rosrun tf2_tools view_frames.py生成一份frame.pdf文件
- evince frame.pdf
- 效果：
