geometry_msgs/TransformStamped
传输坐标系相关位置信息
std_msgs/Header header
uint32 seq
time stamp
string frame_id 被参考坐标系(基坐标系)
string child_frame_id 另外坐标系
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation 平移
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation 坐标A相对于坐标B的欧拉角(三个角)
float64 x 四元数，旋转
float64 y
float64 z
float64 w
geometry_msgs/PointStamped
传输某个坐标系内坐标点的信息
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Point point
float64 x
float64 y
float64 z
静态坐标变换，是指两个坐标系之间的相对位置是固定的。
问题背景：
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
建文件
tf01_static
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs
查看
rostopic list
rostopic echo /tf_static
图形化设置
rviz
transforms:
header:
seq: 0
stamp:
secs: 1658132852
nsecs: 825164513
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
#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::StaticTransformBroadcaster pub;
// 4. 组织被发布的消息，对应头文件3
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::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;
}
#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;
}
#! /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()
#! /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()
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、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
依次是偏航角，俯仰角，翻滚角
偏航：Yaw
俯仰：Pitch
翻滚：Roll
X轴：红色(翻滚角)正值 顺时针转动；负值 逆时针转动
Y轴：绿色(俯仰角)
Z轴：蓝色(偏航角)
话题：/turtle1/pose
消息类型：turtlesim/Pose
消息格式：
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
rosrun turtlesim turtlesim_node
rostopic list
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose 位姿发布使用的话题
turtle1/pose
Type: turtlesim/Pose 消息类型
Publishers: * /turtlesim (http://rosnoetic-VirtualBox:37261/)
Subscribers: None
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
#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;
}
roscore
rosrun turtlesim turtlesim_node
rosrun tf02_dynamic demo01_dynamic_pub
rosrun tf02_dynamic demo02_dynamic_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;
}
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()
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()