文章目录
01 多坐标变换
1.1 需求 - 分析 - 流程
需求描述:
- 现有坐标系统,父级坐标系统 world,下有两子级系统 son1,son2,son1 相对于 world,以及 son2 相对于world 的关系是已知的,求 son1 原点在 son2 中的坐标,又已知在 son1 中一点的坐标,要求求出该点在 son2 中的坐标。
实现分析:
- 首先,需要发布 son1 相对于 world,以及 son2 相对于 world 的坐标消息
- 然后,需要订阅坐标发布消息,并取出订阅的消息,借助于 tf2 实现 son1 和 son2 的转换
- 最后,还要实现坐标点的转换
实现流程:C++ 与 Python 实现流程一致
- 新建功能包,添加依赖
- 创建坐标相对关系发布方(需要发布两个坐标相对关系)
- 创建坐标相对关系订阅方
- 编译执行
1.2 新建功能包,添加依赖 —— 功能包 tf03_tfs
- 新建功能包
tf01_static
- 添加依赖
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim
1.3 程序实现
1.3.1 发布方 —— tfs_c.launch
- 为了方便,使用静态坐标变换发布
<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>
- 启动窗口1
cd demo02_ws/
roslaunch tf03_tfs tfs_c.launch
- 启动窗口2
rviz
- fixed frame 选择是 world,选择 Add 中 TF
1.3.2 编译运行
- 编译
Ctrl + Shift + B
- 启动窗口 1
roscore
- 启动窗口 2
cd demo02_ws/
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch(先启动)
- 启动窗口 3
cd demo02_ws/
source ./devel/setup.bash
rosrun tf03_tfs demo01_tfs_sub(开新的窗口)
son1 相对于 son2 的信息:父级: son2,子级: son1 偏移量(2.00,0.00,0.00)
son1 相对于 son2 的信息:父级: son2,子级: son1
- 启动窗口 4
rviz
1.3.3 订阅方
1.3.3.1 C++实现 —— demo01_tfs.cpp
#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. 创建订阅对象 头文件2/3
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
// 4. 编写解析逻辑
// 创建坐标点 头文件4/5
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));
// 目标坐标系,源坐标系,时间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;
}
1.3.3.2 配置 CMakeLists.txt
add_executable(demo01_tfs src/demo01_tfs.cpp)
add_dependencies(demo01_tfs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(demo01_tfs
${catkin_LIBRARIES}
)
1.3.3.3 Python实现 —— demo01_tfs_p.py
转换后的坐标:(3.00,2.00,3.00),参考的坐标系: son2 父级坐标系: son2,子级坐标系: son1, 偏移量(2.00,0.00,0.00)
#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped
if __name__ == "__main__":
# 2. 初始化
rospy.init_node("tfs_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.stamp = rospy.Time.now() # launch使用的是静态坐标变换
# 坐标系
ps.header.frame_id = "son1"
ps.point.x = 1.0
ps.point.y = 2.0
ps.point.z = 3.0
# 5. 转换逻辑实现,调用tf封装的算法
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 使用try防止
try:
# ------ 需要计算 son1 相对于 son2 的坐标关系 头文件4
"""
参数1: 目标坐标系
参数2: 源坐标系
参数3: rospy.Time(0) --- 取时间间隔最近的两个坐标系帧(son1相对world,与son2相对world)计算
返回值: son1 与 son2坐标系关系
"""
ts = buffer.lookup_transform("son2","son1",rospy.Time(0))
rospy.loginfo("父级坐标系: %s,子级坐标系: %s, 偏移量(%.2f,%.2f,%.2f)",
ts.header.frame_id,
ts.child_frame_id,
ts.transform.translation.x,
ts.transform.translation.y,
ts.transform.translation.z
)
# 转换实现
ps_out = buffer.transform(ps,"son2") # 被转换的坐标点,目标坐标系的名称
# 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.3.3.4 配置 CMakeLists.txt
- 启动窗口1
roscore
- 启动窗口2
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch
- 启动窗口3
rosrun tf2_tools view_frames.py
- 在 vscode 终端里打开:ctrl+`
02 坐标系关系查看
在机器人系统中,涉及的坐标系有多个,为了方便查看,ros 提供了专门的工具,可以用于生成显示坐标系关系的 pdf 文件,该文件包含树形结构的坐标系图谱。
2.1 准备
- 首先调用rospack find tf2_tools查看是否包含该功能包,如果没有,请使用如下命令安装
sudo apt install ros-noetic-tf2-tools
2.2 使用
2.2.1 生成 pdf 文件
- 启动坐标系广播程序之后,运行如下命令:
rosrun tf2_tools view_frames.py
- 会产生类似于下面的日志信息
[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...
- 查看当前目录会生成一个 frames.pdf 文件
2.2.2 查看文件
- 可以直接进入目录打开文件,或者调用命令查看文件:evince frames.pdf( 查看 pdf)