第 5 章 ROS 常用组件 1 —— TF 坐标变换_静态坐标变换 tf01_static / 动态坐标变换 tf02_dynamic
文章目录
- 00 学习目标
- 01 TF 坐标变换
-
- 1.0 概念 - 作用 - 说明
- 1.1 坐标 msg 消息
-
- 1.1.1 geometry_msgs/TransformStamped 坐标系之间位置信息
- 1.1.2 geometry_msgs/PointStamped 坐标系内坐标点
- 1.2 静态坐标变换 —— 功能包 tf01_static
-
- 1.2.0 需求 - 分析 - 流程
- 1.2.1 新建功能包，添加依赖
- 1.2.2 C++实现
-
- 1.2.2.1 发布方 demo01_static_pub.cpp
- 1.2.2.2 配置 CMakeLists.txt
- 1.2.2.3 编译检验 demo01_static_pub.cpp
- 1.2.2.4 订阅方 demo02_static_sub.cpp
- 1.2.2.5 配置 CMakeLists.txt
- 1.2.2.6 发布方与订阅方 编译执行
- 1.2.3 Python实现
-
- 1.2.3.1 发布方 demo01_static_pub_p.py
- 1.2.3.2 订阅方 demo02_static_sub_p.py
- 1.2.4 x - 翻滚 y - 俯仰 z - 偏航(重要!!!)
- 1.2.5 补充讲解
-
- 1.2.5.1 补充1
- 1.2.5.2 补充2
- 1.3 动态坐标变换 —— 功能包 tf02_dynamic
-
- 1.3.0 需求 - 分析 - 流程
- 1.3.1 新建功能包，添加依赖
- 1.3.2 C++实现
-
- 1.3.2.0 预准备 - 获取话题和消息
- 1.3.2.1 发布方 demo01_dynamic_pub.cpp
- 1.3.2.2 配置 CMakeLists.txt
- 1.3.2.3 订阅方 demo02_dynamic_sub.cpp(类似 demo02_static.sub)
- 1.3.2.4 配置 CMakeLists.txt
- 1.3.2.5 编译运行
- 1.3.3 Python实现
-
- 1.3.3.1 发布方 demo01_dynamic_sub_p.py
- 1.3.3.2 订阅方 demo02_dynamic_sub_p.py
00 学习目标
在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件：
- TF坐标变换，实现不同类型的坐标系之间的转换；
- rosbag 用于录制ROS节点的执行过程并可以重放该过程；
- rqt 工具箱，集成了多款图形化的调试工具。
本章预期达成的学习目标：
- 了解 TF 坐标变换的概念以及应用场景
- 能够独立完成TF案例:小乌龟跟随
- 可以使用 rosbag 命令或编码的形式实现录制与回放
- 能够熟练使用rqt中的图形化工具
01 TF 坐标变换
机器人系统上，有多个传感器，如激光雷达、摄像头等，有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标，横向、纵向、高度的距离信息)的，以协助机器人定位障碍物，可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息吗？显示是不行的，这中间需要一个转换过程。更具体描述如下：
场景1:雷达与小车
现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？
场景2：现有一带机械臂的机器人(比如:PR2)需要夹取目标物，当前机器人头部摄像头可以探测到目标物的坐标(x,y,z)，不过该坐标是以摄像头为参考系的，而实际操作目标物的是机械臂的夹具，当前我们需要将该坐标转换成相对于机械臂夹具的坐标，这个过程如何实现？
- 窗口1：第2只乌龟会跟随第1只乌龟运动
roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch（C++实现)
roslaunch turtle_tf2 turtle_tf2_demo.launch（python实现)
1.0 概念 - 作用 - 说明
概念
- tf：TransForm Frame 坐标变换
- 坐标系：ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的
作用 - 在 ROS 中用于实现不同坐标系之间的点或向量的转换
说明
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有：
- tf2_geometry_msgs：可以将ROS消息转换成tf2消息
- tf2：封装了坐标变换的常用消息
- tf2_ros：为 tf2 提供了 roscpp 和 rospy 绑定，封装了坐标变换常用的 API
官方参考
1.1 坐标 msg 消息
订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg：
- geometry_msgs/TransformStamped：用于传输坐标系相关位置信息
- geometry_msgs/PointStamped：用于传输某个坐标系内坐标点的信息
在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。
1.1.1 geometry_msgs/TransformStamped 坐标系之间位置信息
- 命令行键入：rosmsg info geometry_msgs/TransformStamped，传输坐标系相关位置信息
名词解释
- string frame_id 被参考坐标系(基坐标系)，string child_frame_id 另外坐标系
- geometry_msgs/Quaternion rotation 坐标 A 相对于坐标 B 的欧拉角(三个角)
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
- 四元数用于表示坐标的相对姿态
1.1.2 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
1.2 静态坐标变换 —— 功能包 tf01_static
- 静态坐标变换，是指两个坐标系之间的相对位置是固定的。
1.2.0 需求 - 分析 - 流程
需求
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下：x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
分析
- 坐标系相对关系，可以通过发布方发布
- 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出
流程：C++ 与 Python 实现流程一致
- 新建功能包，添加依赖
- 编写发布方实现
- 编写订阅方实现
- 执行并查看结果
1.2.1 新建功能包，添加依赖
- 新建功能包
tf01_static
- 添加依赖
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs
1.2.2 C++实现
1.2.2.1 发布方 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"
/*
需求： 发布两个坐标系的相对关系
流程：
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. 创建发布对象 对应头文件2 —— tf2_ros/static_transform_broadcaster.h 静态坐标转换广播器
tf2_ros::StaticTransformBroadcaster pub;
// 4. 组织被发布的消息 对应头文件3 —— geometry_msgs/TransformStamped.h
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;//雷达x偏移值
tfs.transform.translation.y = 0.0;//雷达y偏移值
tfs.transform.translation.z = 0.5;//雷达z偏移值
// 4.1 需要根据欧拉角转换 对应头文件4 —— tf2/LinearMath/Quaternion.h
tf2::Quaternion qtn; // 创建四元数对象
// 4.2 向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数
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;
}
1.2.2.2 配置 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}
)
1.2.2.3 编译检验 demo01_static_pub.cpp
- 编译
Ctrl + Shift + B
检验方式1
- 窗口1：挂起
roscore
cd demo02_ws/
source ./devel/setup.bash
rosrun tf01_static demo01_static_pub
- 窗口2
rostopic list
- 打印该话题下的消息
rostopic echo /tf_static
检验方式2
- 窗口1：图形化设置
rviz
- Fixed Frame 选择 base_link
- 点击左下角 Add，选择 TF
- 红色 x 轴，绿色 y 轴，蓝色 z 轴
1.2.2.4 订阅方 demo02_static_sub.cpp
- 核心代码
// 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5 —— tf2_geometry_msgs/tf2_geometry_msgs.h
geometry_msgs::PointStamped ps_out;
/*
调用buffer(数据存在缓存buffer里面)的转换函数 transform
参数1: 被转换的坐标点
参数2: 目标坐标系
返回值: 输出的坐标点
注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
注意2: 运行时存在的问题，抛出一个异常 base_link不存在
原因:
订阅数据是一个耗时操作，可能调用transform转换函数时，
坐标系相对关系还没有订阅到，因此出现异常
解决:
方案1: 在调用转换函数时，执行休眠
方案2: 进行异常处理
*/
ps_out = buffer.transform(ps,"base_link"); // 目标点信息，基参考系，输出转换后的坐标
- 订阅方程序
#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. 创建订阅对象: ---> 订阅坐标系相对关系 头文件2\3
// 3.1 创建一个buffer缓存
tf2_ros::Buffer buffer;
// 3.2 再创建监听对象(监听对象可以将订阅的数据存入buffer)
tf2_ros::TransformListener listener(buffer);
// 4. 组织一个坐标点数据 头文件4
geometry_msgs::PointStamped ps;
ps.header.stamp = ros::Time::now();
ps.header.frame_id = "laser";// 坐标系 障碍物位置相对于雷达坐标系而言
ps.point.x = 2.0; // 障碍物相对于雷达坐标系的位置
ps.point.y = 3.0;
ps.point.z = 5.0;
// 添加休眠 防止订阅方没有订阅到发布方消息
ros::Duration(2).sleep();
// 5. 转换算法，需要调用tf内置实现
ros::Rate rate(1);
while(ros::ok())
{
// 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5
geometry_msgs::PointStamped ps_out;
/*
调用buffer(数据存在缓存buffer里面)的转换函数 transform
参数1: 被转换的坐标点
参数2: 目标坐标系
返回值: 输出的坐标点
注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
注意2: 运行时存在的问题，抛出一个异常 base_link不存在
原因:
订阅数据是一个耗时操作，可能调用transform转换函数时，
坐标系相对关系还没有订阅到，因此出现异常
解决:
方案1: 在调用转换函数时，执行休眠
方案2: 进行异常处理
*/
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.header.frame_id.c_str()
);
rate.sleep();
ros::spinOnce();
}
return 0;
}
1.2.2.5 配置 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}
)
1.2.2.6 发布方与订阅方 编译执行
- 注意：在订阅方程序中增加延时，保证先发布，再订阅，防止报错
- 编译
Ctrl + Shift + B
- 窗口1
roscore
- 启动发布方 窗口2
cd demo02_ws/
source ./devel/setup.bash
rosrun tf01_static demo01_static_pub
- 启动订阅方 窗口3
cd demo02_ws/
source ./devel/setup.bash
rosrun tf01_static demo02_static_sub
1.2.3 Python实现
1.2.3.1 发布方 demo01_static_pub_p.py
#! /usr/bin/env python
"""
发布方: 发布两个坐标系的相对关系(车辆底盘 --- base_link 和 雷达 --- laser)
流程:
1. 导包
2. 初始化节点
3. 创建发布对象
4. 组织被发布的数据
5. 发布数据
6. spin()
"""
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
if __name__ == "__main__":
# 2. 初始化节点
rospy.init_node("start_pub_p")
# 3. 创建发布对象 头文件2
pub = tf2_ros.StaticTransformBroadcaster()
# 4. 组织被发布的数据 头文件4
tfs = TransformStamped()
# 4.1 header
tfs.header.stamp = rospy.Time.now()
tfs.header.frame_id = "base_link"
# 4.2 child frame
tfs.child_frame_id = "laser"
# 4.3 相对关系(偏移 与 四元数)
tfs.transform.translation.x = 0.2
tfs.transform.translation.y = 0.0
tfs.transform.translation.z = 0.5
# 4.4 先从欧拉角转换成四元数 头文件3
qtn = tf.transformations.quaternion_from_euler(0,0,0)
# 4.5 再设置四元数
tfs.transform.rotation.x = qtn[0]
tfs.transform.rotation.y = qtn[1]
tfs.transform.rotation.z = qtn[2]
tfs.transform.rotation.w = qtn[3]
# 5. 发布数据
pub.sendTransform(tfs)
# 6. spin()
rospy.spin()
1.2.3.2 订阅方 demo02_static_sub_p.py
#! /usr/bin/env python
"""
订阅方: 订阅坐标变换消息，传入被转换的坐标，调用转换算法
流程:
1. 导包
2. 初始化
3. 创建订阅对象
4. 组织被转换的坐标点
5. 转换逻辑实现，调用tf封装的算法
6. 输出结果
"""
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
if __name__ == "__main__":
# 2. 初始化
rospy.init_node("static_sub_p")
# 3. 创建订阅对象
# 3.1 创建一个缓存对象 头文件2
buffer = tf2_ros.Buffer()
# 3.2 创建订阅对象(将缓存传入)
sub = tf2_ros.TransformListener(buffer)
# 4. 组织被转换的坐标点 头文件3
ps = tf2_geometry_msgs.PointStamped()
ps.header.stamp = 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(1)
while not rospy.is_shutdown():
# 使用try防止
try:
# 转换实现
"""
参数1: 被转换的坐标点
参数2: 目标坐标系
返回值: 转换后的坐标点
PS:
问题: 抛出异常 base_link 不存在
原因: 转换函数调用时，可能还没有订阅到坐标系的相对关系
解决: 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 Exception as e:
rospy.logwarn("错误提示:%s",e)
rate.sleep()
1.2.4 x - 翻滚 y - 俯仰 z - 偏航(重要!!!)
- 当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y 俯仰角度、z 偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，摄像头相对于基坐标系位置。
- rosrun tf2_ros static_transform_publisher 0.1 0.0 0.3 0 0 0 /base_link /camera
- rviz：依次是z - 偏航角(Yaw)，y - 俯仰角(Pitch)，x 翻滚角(Roll)
- X轴：红色(翻滚角) 正值 —— 顺时针转动；负值 —— 逆时针转动
- Y轴：绿色(俯仰角)
- Z轴：蓝色(偏航角)
1.2.5 补充讲解
1.2.5.1 补充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 /baselink /laser
也建议使用该种方式直接实现静态坐标系相对信息发布。
1.2.5.2 补充2
可以借助于rviz显示坐标系关系，具体操作：
- 新建窗口输入命令：rviz
- 在启动的 rviz 中设置Fixed Frame 为 base_link
- 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系
1.3 动态坐标变换 —— 功能包 tf02_dynamic
1.3.0 需求 - 分析 - 流程
所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。
需求描述
- 启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
实现分析
- 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
- 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
- 将 pose 信息转换成 坐标系相对信息并发布
实现流程：C++ 与 Python 实现流程一致
- 新建功能包，添加依赖
- 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
- 创建坐标相对关系订阅方
- 编译执行
1.3.1 新建功能包，添加依赖
- 新建功能包
tf02_dynamic
- 添加依赖
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim
1.3.2 C++实现
1.3.2.0 预准备 - 获取话题和消息
- 话题：/turtle1/pose 消息类型：turtlesim/Pose（类似于Person.msg）
- 启动窗口1
roscore
- 启动窗口2 —— 打开乌龟节点
rosrun turtlesim turtlesim_node
- 启动窗口3 —— 获取话题 /turtle1/pose
rostopic list
启动窗口4 —— 获取消息格式 turtlesim/Pose
rostopic info /turtle1/pose
- 启动窗口5 —— 获取消息的具体格式
rosmsg info turtlesim/Pose
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
1.3.2.1 发布方 demo01_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"
/*
发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系，并发布
准 备:
话题： /turtle1/pose
消息： turtlesim/Pose
流程:
1. 包含头文件
2. 设置编码，初始化， NodeHandle
3. 创建订阅对象，订阅/turtle1/pose
4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
5. spin()
*/
// 给传进回调函数的数据定义范型 头文件2
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 获取位姿信息，转换成坐标系相对关系(核心)，并发布
// a. 创建发布对象 头文件3
static tf2_ros::TransformBroadcaster pub;
// b. 组织被发布的数据 头文件4
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）
// 坐标系四元数 头文件5
/*
位姿信息中没有四元数，但是有偏航角度，又已知乌龟时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("/turtle1/pose",100,doPose);
// 4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
// 5. spin()
ros::spin();
return 0;
}
1.3.2.2 配置 CMakeLists.txt
add_executable(demo01_dynamic_pub src/demo01_dynamic_pub.cpp)
add_dependencies(demo01_dynamic_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(demo01_dynamic_pub
${catkin_LIBRARIES}
)
1.3.2.3 订阅方 demo02_dynamic_sub.cpp(类似 demo02_static.sub)
#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. 创建订阅对象: ---> 订阅坐标系相对关系 头文件2\3
// 3.1 创建一个buffer缓存
tf2_ros::Buffer buffer;
// 3.2 再创建监听对象(监听对象可以将订阅的数据存入buffer)
tf2_ros::TransformListener listener(buffer);
// 4. 组织一个坐标点数据 头文件4
geometry_msgs::PointStamped ps;
// 参考的坐标系
ps.header.frame_id = "turtle1";// 坐标系 物体位置相对于乌龟坐标系而言
// 时间戳
ps.header.stamp = ros::Time(0.0);
ps.point.x = 2.0; // 物体相对于乌龟坐标系的位置
ps.point.y = 3.0;
ps.point.z = 5.0;
// 添加休眠 防止订阅方没有订阅到发布方消息(这个方法不建议用，最好使用try方法
// ros::Duration(2).sleep();
// 5. 转换算法，需要调用tf内置实现
ros::Rate rate(1);
while(ros::ok())
{
// 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5
geometry_msgs::PointStamped ps_out;
/*
调用buffer(数据存在缓存buffer里面)的转换函数 transform
参数1: 被转换的坐标点
参数2: 目标坐标系
返回值: 输出的坐标点
注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
注意2: 运行时存在的问题，抛出一个异常 base_link不存在
原因:
订阅数据是一个耗时操作，可能调用transform转换函数时，
坐标系相对关系还没有订阅到，因此出现异常
解决:
方案1: 在调用转换函数时，执行休眠
方案2: 进行异常处理
*/
try
{
ps_out = buffer.transform(ps,"world"); // 目标点信息，基参考系，输出转换后的坐标 头文件5r
// 6. 最后输出
ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps.header.frame_id.c_str()
);
}
catch(const std::exception& e)
{
ROS_INFO("异常消息:%s",e.what());
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
1.3.2.4 配置 CMakeLists.txt
add_executable(demo02_dynamic_sub src/demo02_dynamic_sub.cpp)
add_dependencies(demo02_dynamic_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(demo02_dynamic_sub
${catkin_LIBRARIES}
)
1.3.2.5 编译运行
- 编译
Ctrl + Shift + B
- 启动窗口1
roscore
- 启动窗口2
rosrun turtlesim turtlesim_node
- 启动发布方 窗口3
cd demo02_ws/
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub
- 启动订阅方 窗口4
cd demo02_ws/
source ./devel/setup.bash
rosrun tf02_dynamic demo02_dynamic_sub
- 启动窗口5：启动乌龟节点，使用键盘控制乌龟运动，对应转换的坐标值也在变化
cd demo02_ws/
source ./devel/setup.bash
rosrun turtlesim turtle_teleop_key
1.3.3 Python实现
1.3.3.1 发布方 demo01_dynamic_sub_p.py
roscore
rosrun turtlesim turtlesim_node
cd demo02_ws/
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub_p.py
/rosout
/rosout_agg
/tf
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
rostopic echo /tf 查看具体信息
ransforms:
header:
seq: 0
stamp:
secs: 1658287751
nsecs: 917598485
frame_id: “world”
child_frame_id: “turtle1”
transform:
translation:
x: 5.544444561004639
y: 5.544444561004639
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
#! /usr/bin/env python
"""
发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系，并发布
准 备:
话题： /turtle1/pose
消息： turtlesim/Pose
流程:
1. 包含头文件
2. 初始化ROS节点
3. 创建订阅对象，订阅/turtle1/pose
4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
5. spin()
"""
import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
def doPose(pose):
# 1. 创建发布坐标系相对关系的对象 头文件3
pub = tf2_ros.TransformBroadcaster()
# 2. 将 pose 转换成 坐标系相对关系消息 头文件4
ts = TransformStamped()
ts.header.frame_id = "world"
ts.header.stamp = rospy.Time.now()
ts.child_frame_id = "turtle1"
# 2.1 子级坐标系相对于父级坐标系的偏移量
ts.transform.translation.x = pose.x
ts.transform.translation.y = pose.y
ts.transform.translation.z = 0
# 2.2 四元数 头文件5
# 从欧拉角转换四元数
"""
乌龟是2D，不存在 x 上的翻滚和 y 上的俯仰，只有 z 上的偏航
0 0 pose.theta
"""
qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
ts.transform.rotation.x = qtn[0]
ts.transform.rotation.y = qtn[1]
ts.transform.rotation.z = qtn[2]
ts.transform.rotation.w = qtn[3]
# 3. 发布
pub.sendTransform(ts)
if __name__ == "__main__":
# 2. 初始化ROS节点
rospy.init_node("dynamic_pub_p")
# 3. 创建订阅对象 头文件2
sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size = 100)# /tur是话题名称
# 4. 回调函数处理订阅信息
# 5. spin()
rospy.spin()
1.3.3.2 订阅方 demo02_dynamic_sub_p.py
roscore
cd demo02_ws/
rosrun turtlesim turtlesim_node
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub_p.py
rosrun tf02_dynamic demo02_dynamic_sub_p.py
控制乌龟运动
rosrun turtlesim turtle_teleop_key
#! /usr/bin/env python
"""
订阅方: 订阅坐标变换消息，传入被转换的坐标，调用转换算法
流程:
1. 导包
2. 初始化
3. 创建订阅对象
4. 组织被转换的坐标点
5. 转换逻辑实现，调用tf封装的算法
6. 输出结果
"""
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
if __name__ == "__main__":
# 2. 初始化
rospy.init_node("dynamic_sub_p")
# 3. 创建订阅对象
# 3.1 创建一个缓存对象 头文件2
buffer = tf2_ros.Buffer()
# 3.2 创建订阅对象(将缓存传入)
sub = tf2_ros.TransformListener(buffer)
# 4. 组织被转换的坐标点 头文件3
ps = tf2_geometry_msgs.PointStamped()
# 时间戳--0
ps.header.stamp = rospy.Time()
# 坐标系
ps.header.frame_id = "turtle1"
ps.point.x = 2.0
ps.point.y = 3.0
ps.point.z = 5.0
# 5. 转换逻辑实现，调用tf封装的算法
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 使用try防止
try:
# 转换实现
"""
参数1: 被转换的坐标点
参数2: 目标坐标系
返回值: 转换后的坐标点
PS:
问题: 抛出异常 base_link 不存在
原因: 转换函数调用时，可能还没有订阅到坐标系的相对关系
解决: try 捕获异常，并处理
"""
ps_out = buffer.transform(ps,"world")
# 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 Exception as e:
rospy.logwarn("错误提示:%s",e)
rate.sleep()