在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件:
TF坐标变换，实现不同类型的坐标系之间的转换；
rosbag 用于录制ROS节点的执行过程并可以重放该过程；
rqt工具箱，集成了多款图形化的调试工具。
---------------------------------------------------------------------------------------------------------------------------------
TF坐标变换
坐标系：ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
tf2: 封装了坐标变换的常用消息。
tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
---------------------------------------------------------------------------------------------------------------------------------
坐标msg信息
订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransforStamped和geometry_msgs/PointStamped，前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。
1.geometry_msgs/TransforStamped
$rosmsg info geometry_msgs/TransforStamped
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 w2.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---------------------------------------------------------------------------------------------------------------------------------
静态坐标变换
静态坐标变换，是指两个坐标系之间的相对位置是固定的。
需求描述：现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
实现分析：
1.坐标系相对关系，可以通过发布方发布
2.订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出
实现流程：
1.新建功能包，添加依赖
2.编写发布方实现
3.编写订阅方实现
4.执行并查看结果
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;
}
$rosrun 功能包名 节点名
$rviz 打开ros自带软件rviz，将上述代码图片可视化
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：目标坐标系
返回值：输出的坐标点
PS1：调用时必须包含头文件tf2_geometry_msgs/tf2_geometry_msgs.h
PS2:运行时存在的问题：抛出一个异常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：
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、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 /laser
也建议使用该种方式直接实现静态坐标系相对信息发布。
---------------------------------------------------------------------------------------------------------------------------------
动态坐标变换
动态坐标变换，是指两个坐标系之间的相对位置是变化的。
需求描述：启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
实现分析：
1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2.订阅/turtle/pose，可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3.将pose信息转换成 坐标系相对信息并发布
实现流程：
1.新建功能包，添加依赖
2.创建坐标相对关系发布方（同时需要订阅乌龟位姿信息）
3.创建坐标相对关系订阅方
4.执行
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"
/*
发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系(rviz)，并发布
准备：
话题：/turtle1/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<turtlesim::Pose>("/turtle1/pose",100,doPose);
// 4.回调函数处理订阅的消息；将位姿信息转换成坐标相对关系并发布(关注)
// 5.spin()
ros::spin();
return 0;
}
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,"dynamic_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=0;
ps.point.y=0;
ps.point.z=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：目标坐标系
返回值：输出的坐标点
PS1：调用时必须包含头文件tf2_geometry_msgs/tf2_geometry_msgs.h
PS2:运行时存在的问题：抛出一个异常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;
}
注意：ps的x,y,z的三个数据设置不同，运行时产生的结果也会不同。输出结果不同是因为发布方中的坐标对应关系在发生变化。
---------------------------------------------------------------------------------------------------------------------------------多坐标转换
需求描述：现有坐标系统，父级坐标系统world下，有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标。
实现分析：
1.首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
2.然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
3.最后，还要实现坐标点的转换
实现流程：
1.新建功能包，添加依赖
2.创建坐标相对关系发布方(需要发布两个坐标相对关系)
3.创建坐标相对关系订阅方
4.执行
1.创建功能包
创建项目功能包依赖于tf2、tf2_ros、tf2_geometry_msgs、roscpp、rospy、std_msgs、geometry_msgs
2.发布方
为了方便，使用静态坐标变换发布
<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>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"
#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：目标坐标系 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);
//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;
}
--------------------------------------------------------------------------------------------------------------------------------坐标系关系查看
ros提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。
首先调用$rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:
$sudo apt install ros-noetic-tf2-tools 其中的noetic需要替换成自己ros的版本
1.生成pdf文件
启动坐标系广播程序之后，运行如下命令:
rosrun tf2_tools view_frames.py会产生类似于下面的日志信息：
查看当前目录会生成一个frames.pdf文件
2.查看文件
可以直接进入目录打开文件，或者调用命令查看文件:$evince frames.pdf
扫一扫
1136
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
