【ROS】—— ROS常用组件_TF坐标变换_静态坐标变换与动态坐标变换(十)
文章目录
- 前言
-
- 1. 坐标msg消息
- 1.1 geometry_msgs/TransformStamped
- 1.2 geometry_msgs/PointStamped
- 2. 静态坐标变换
-
- 2.1 C++实现
-
- 2.1.1 发布方
- 2.1.2 订阅方
- 2.2 python实现
-
- 2.2.1 发布方
- 2.2.2 订阅方
- 2.3 补充
- 3. 动态坐标变换
-
- 3.1 C++实现
-
- 3.1.1 发布方
- 3.1.2 订阅方
- 3.2 python实现
-
- 3.2.1 发布方
- 3.2.2 订阅方
前言
本系列将依托赵虚左老师的ROS课程，写下自己的一些心得与笔记。
课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
讲义链接:http://www.autolabor.com.cn/book/ROSTutorials/index.html
文章可能存在疏漏的地方，恳请大家指出。
ROS中内置有小乌龟跟随实践案例,输入以下命令
roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch或roslaunch turtle_tf2 turtle_tf2_demo.launch
带cpp的是用C++实现的,另一个则是python实现的.
概念
tf: TransForm Frame,坐标变换
坐标系: ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。
作用
在 ROS 中用于实现不同坐标系之间的点或向量的转换。
说明
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:
tf2_geometry_msgs: 可以将ROS消息转换成tf2消息。
tf2: 封装了坐标变换的常用消息。
tf2_ros: 为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
1. 坐标msg消息
订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。
1.1 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 #|-- X 方向的偏移量
float64 y #|-- Y 方向的偏移量
float64 z #|-- Z 方向上的偏移量
geometry_msgs/Quaternion rotation #四元数
float64 x
float64 y
float64 z
float64 w
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
2. 静态坐标变换
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
需求描述:
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
实现分析:
- 坐标系相对关系，可以通过发布方发布
- 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出
实现流程: C++ 与 Python 实现流程一致
-
新建功能包，添加依赖
-
编写发布方实现
-
编写订阅方实现
-
执行并查看结果
2.1 C++实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.1.1 发布方
#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); //(roll pitch yaw)欧拉角的单位为弧度
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 echo /tf_static
transforms:
header:
seq: 0
stamp:
secs: 1673180975
nsecs: 668421241
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
运行rivz查看坐标系相对关系,Fixed Frame设置为base_link
rosrun rviz rviz
2.1.2 订阅方
#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"
//transform_listener.h订阅数据,buffer.h缓存(将订阅的数据存储至缓存中)
/*
订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 TF 订阅节点
4.生成一个坐标点(相对于子级坐标系)
5.转换坐标点(相对于父级坐标系),调用tf内置功能实现
6.spin()
*/
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"static_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
//创建一个buffer缓存
tf2_ros::Buffer buffer;
//再创建监听对象(监听对象可以将订阅的数据存入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;
// 解决方案1:添加休眠
ros::Duration(2).sleep();
//terminate called after throwing an instance of 'tf2::LookupException'
// what(): "base_link" passed to lookupTransform argument target_frame does not exist.
// 已放弃 (核心已转储)
//原因: 发布的坐标系相对关系未拿到坐标点却开始发生转换
// 解决方案1:添加休眠
// 5.转换坐标点(相对于父级坐标系)
ros::Rate rate(10);
while (ros::ok())
{
//将ps转化为相对于base_link的坐标点
geometry_msgs::PointStamped ps_out;
ps_out = buffer.transform(ps,"base_link");
ROS_INFO("转换后的坐标值:(x: %0.2f , y: %0.2f ,z: %0.2f),参考坐标系%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str()
);
// 6.spin()
rate.sleep();
ros::spinOnce();
}
return 0;
}
rosrun tf2_test demo01_static_pub
rosrun tf2_test demo02_static_sub
出现以下问题
terminate called after throwing an instance of 'tf2::LookupException'
what(): "base_link" passed to lookupTransform argument target_frame does not exist.
已放弃 (核心已转储)
原因: 发布的坐标系相对关系未拿到坐标点却开始发生转换
解决方案1:添加休眠
解决方案2:异常处理(建议)
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"base_link");
ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常.....");
}
运行成功
PS: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h,否则编译异常.
2.2 python实现
实现流程与C++基本一致
2.2.1 发布方
#! /usr/bin/env python
#-- coding:UTF-8 --
"""
静态坐标变换发布方:
发布关于 laser 坐标系的位置信息
实现流程:
1.导包
2.初始化 ROS 节点
3.创建 静态坐标广播器
4.创建并组织被广播的消息
5.广播器发送消息
6.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
if __name__ == "__main__":
# 2.初始化 ROS 节点
rospy.init_node("static_tf_pub_p")
# 3.创建 静态坐标广播器
broadcaster = tf2_ros.StaticTransformBroadcaster()
# 4.创建并组织被广播的消息
tfs = TransformStamped()
# --- 头信息
tfs.header.frame_id = "world"
tfs.header.stamp = rospy.Time.now()
tfs.header.seq = 101
# --- 子坐标系
tfs.child_frame_id = "radar"
# --- 坐标系相对信息
# ------ 偏移量
tfs.transform.translation.x = 0.2
tfs.transform.translation.y = 0.0
tfs.transform.translation.z = 0.5
# ------ 四元数
qtn = tf.transformations.quaternion_from_euler(0,0,0)
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.广播器发送消息
broadcaster.sendTransform(tfs)
# 6.spin
rospy.spin()
rviz显示
2.2.2 订阅方
#! /usr/bin/env python
#-- coding:UTF-8 --
"""
订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，
转换成父级坐标系中的坐标点
实现流程:
1.导包
2.初始化 ROS 节点
3.创建 TF 订阅对象
4.创建一个 radar 坐标系中的坐标点
5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
6.spin
"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
if __name__ == "__main__":
# 2.初始化 ROS 节点
rospy.init_node("static_sub_tf_p")
# 3.创建 TF 订阅对象
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 4.创建一个 radar 坐标系中的坐标点
point_source = PointStamped()
point_source.header.frame_id = "radar"
point_source.header.stamp = rospy.Time.now()
point_source.point.x = 10
point_source.point.y = 2
point_source.point.z = 3
try:
# 5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
point_target = buffer.transform(point_source,"world")
rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
point_target.point.x,
point_target.point.y,
point_target.point.z)
except Exception as e:
rospy.logerr("异常:%s",e)
# 6.spin
rate.sleep()
2.3 补充
补充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
也建议使用该种方式直接实现静态坐标系相对信息发布。
3. 动态坐标变换
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
实现分析:
- 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
- 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
- 将 pose 信息转换成 坐标系相对信息并发布
实现流程: C++ 与 Python 实现流程一致
-
新建功能包，添加依赖
-
创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
-
创建坐标相对关系订阅方
-
执行
3.1 C++实现
3.1.1 发布方
#include "ros/ros.h"
#include"turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
控制乌龟运动，将两个坐标系的相对位置动态发布
实现分析:
1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3.将 pose 信息转换成 坐标系相对信息并发布
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 ROS 句柄
4.创建订阅对象
5.回调函数处理订阅到的数据(实现TF广播)
5-1.创建 TF 广播器
5-2.创建 广播的数据(通过 pose 设置)
5-3.广播器发布数据
6.spin
*/
void doPose(const turtlesim::Pose::ConstPtr& pose)
{
//获取位姿信息,转换并发布
//1.创建发布对象
static tf2_ros::TransformBroadcaster broadcaster ;
//2.组织被发布的数据
geometry_msgs::TransformStamped tfs;
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
tfs.child_frame_id = "turtle1";
//偏移量,从pose中获取
/*
位姿信息中,没有四元数,但有偏航角度.又因为乌龟是2D的,没有翻滚与俯仰角度,所以可以设置为0.
综上,乌龟的欧拉角为0 0 theta
*/
tfs.transform.translation.x = pose ->x;
tfs.transform.translation.y = pose ->y;
tfs.transform.translation.z = 0;
//四元数
tf2::Quaternion qtn;
qtn.setRPY(0,0, pose ->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w= qtn.getW();
//3.发布
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_pub");
// 3.创建 ROS 句柄
ros::NodeHandle nh;
// 4.创建订阅对象
ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
// 5.回调函数处理订阅到的数据(实现TF广播)
// 5-1.创建 TF 广播器
// 5-2.创建 广播的数据(通过 pose 设置)
// 5-3.广播器发布数据
// 6.spin
ros::spin();
return 0;
}
rviz效果
3.1.2 订阅方
订阅时,静态坐标变换与动态坐标变换差别不大.
#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"
//transform_listener.h订阅数据,buffer.h缓存(将订阅的数据存储至缓存中)
/*
订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 TF 订阅节点
4.生成一个坐标点(相对于子级坐标系)
5.转换坐标点(相对于父级坐标系),调用tf内置功能实现
6.spin()
*/
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
//创建一个buffer缓存
tf2_ros::Buffer buffer;
//再创建监听对象(监听对象可以将订阅的数据存入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 = 2.0;
ps.point.y = 3.0;
ps.point.z = 5.0;
// 解决方案1:添加休眠
ros::Duration(2).sleep();
//terminate called after throwing an instance of 'tf2::LookupException'
// what(): "base_link" passed to lookupTransform argument target_frame does not exist.
// 已放弃 (核心已转储)
//原因: 发布的坐标系相对关系未拿到坐标点却开始发生转换
// 解决方案1:添加休眠
// 5.转换坐标点(相对于父级坐标系)
ros::Rate rate(10);
while (ros::ok())
{
//将ps转化为相对于base_link的坐标点
geometry_msgs::PointStamped ps_out;
ps_out = buffer.transform(ps,"world");
ROS_INFO("转换后的坐标值:(x: %0.2f , y: %0.2f ,z: %0.2f),参考坐标系%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str()
);
// 6.spin()
rate.sleep();
ros::spinOnce();
}
return 0;
}
日志消息
[ INFO] [1673260912.458748475]: 转换后的坐标值:(x: 6.02 , y: 10.50 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.558674506]: 转换后的坐标值:(x: 6.68 , y: 10.30 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.658733414]: 转换后的坐标值:(x: 7.39 , y: 9.91 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.758674736]: 转换后的坐标值:(x: 7.91 , y: 9.45 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.858798948]: 转换后的坐标值:(x: 8.34 , y: 8.91 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.958771421]: 转换后的坐标值:(x: 8.65 , y: 8.29 ,z: 5.00),参考坐标系world
[ INFO] [1673260913.058666995]: 转换后的坐标值:(x: 8.86 , y: 7.52 ,z: 5.00),参考坐标系world
[ INFO] [1673260913.158740752]: 转换后的坐标值:(x: 8.97 , y: 7.19 ,z: 5.00),参考坐标系world
[ INFO] [1673260913.258723715]: 转换后的坐标值:(x: 9.09 , y: 7.04 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.658674377]: 转换后的坐标值:(x: 10.84 , y: 4.84 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.758673053]: 转换后的坐标值:(x: 10.93 , y: 4.83 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.858675805]: 转换后的坐标值:(x: 10.79 , y: 5.50 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.958716544]: 转换后的坐标值:(x: 10.51 , y: 6.14 ,z: 5.00),参考坐标系world
[ INFO] [1673260915.058708439]: 转换后的坐标值:(x: 10.05 , y: 6.80 ,z: 5.00),参考坐标系world
[ INFO] [1673260915.158602451]: 转换后的坐标值:(x: 9.71 , y: 7.06 ,z: 5.00),参考坐标系world
[ INFO] [1673260915.258678036]: 转换后的坐标值:(x: 9.52 , y: 7.09 ,z: 5.00),参考坐标系world
注意
ps.header.stamp = ros::Time(0,0);不能用原来的 tfs.header.stamp = ros::Time::now();.
原因:使用 ros::Time::now();时因为在动态坐标变换中,buffer缓存中会有许多值,这些值是由发布方不停发布的,且每次发布时,具有不同的时间戳.在进行坐标转换时,会将坐标点的时间戳和坐标关系的时间戳进行比对,若两者之间差距较小,则没什么大问题;若差距较大,则会抛异常.
3.2 python实现
3.2.1 发布方
#! /usr/bin/env python
#-- coding:UTF-8 --
"""
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
控制乌龟运动，将两个坐标系的相对位置动态发布
实现分析:
1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3.将 pose 信息转换成 坐标系相对信息并发布
实现流程:
1.导包
2.初始化 ROS 节点
3.订阅 /turtle1/pose 话题消息
4.回调函数处理
4-1.创建 TF 广播器
4-2.创建 广播的数据(通过 pose 设置)
4-3.广播器发布数据
5.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
# 4.回调函数处理
def doPose(pose):
# 4-1.创建 TF 广播器
broadcaster = tf2_ros.TransformBroadcaster()
# 4-2.创建 广播的数据(通过 pose 设置)
tfs = TransformStamped()
tfs.header.frame_id = "world"
tfs.header.stamp = rospy.Time.now()
tfs.child_frame_id = "turtle1"
tfs.transform.translation.x = pose.x
tfs.transform.translation.y = pose.y
tfs.transform.translation.z = 0.0
qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
tfs.transform.rotation.x = qtn[0]
tfs.transform.rotation.y = qtn[1]
tfs.transform.rotation.z = qtn[2]
tfs.transform.rotation.w = qtn[3]
# 4-3.广播器发布数据
broadcaster.sendTransform(tfs)
if __name__ == "__main__":
# 2.初始化 ROS 节点
rospy.init_node("dynamic_tf_pub_p")
# 3.订阅 /turtle1/pose 话题消息
sub = rospy.Subscriber("/turtle1/pose",Pose,doPose)
# 4.回调函数处理
# 4-1.创建 TF 广播器
# 4-2.创建 广播的数据(通过 pose 设置)
# 4-3.广播器发布数据
# 5.spin
rospy.spin()
3.2.2 订阅方
#! /usr/bin/env python
#-- coding:UTF-8 --
"""
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
控制乌龟运动，将两个坐标系的相对位置动态发布
实现分析:
1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3.将 pose 信息转换成 坐标系相对信息并发布
实现流程:
1.导包
2.初始化 ROS 节点
3.创建 TF 订阅对象
4.处理订阅的数据
"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
if __name__ == "__main__":
# 2.初始化 ROS 节点
rospy.init_node("static_sub_tf_p")
# 3.创建 TF 订阅对象
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 4.创建一个 radar 坐标系中的坐标点
point_source = PointStamped()
point_source.header.frame_id = "turtle1"
point_source.header.stamp = rospy.Time.now()
point_source.point.x = 10
point_source.point.y = 2
point_source.point.z = 3
try:
# 5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
point_target = buffer.transform(point_source,"world",rospy.Duration(1))
rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
point_target.point.x,
point_target.point.y,
point_target.point.z)
except Exception as e:
rospy.logerr("异常:%s",e)
# 6.spin
rate.sleep()
