ROS常用组件---静态坐标变换---动态坐标变换
TF坐标变换
用于传输某个坐标系内坐标点的信息
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
坐标系相对关系：用于传输坐标系相关位置信息
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 #四元数， child_frame_id 相对frame_id 的偏航，俯仰，翻滚，角度上的变化
float64 x
float64 y
float64 z
float64 w
静态坐标变换
创建依赖函数
roscpp rospy std_msgs tf2 tf2_geometry_msgs tf2_ros geometry_msgs
使用c++创建发布方
/*
静态坐标变换发布方:
发布关于 laser 坐标系的位置信息
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建静态坐标转换广播器
4.创建坐标系信息
5.广播器发布坐标系信息
6.spin()
*/${PROJECT_NAME}_node
// 1.包含头文件
#include "ros/ros.h"
#include "tf2_${PROJECT_NAME}_noderos/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"//
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"static_brocast");
// 3.创建静态坐标转换广播器
tf2_ros::StaticTransformBroadcaster broadcaster;
// 4.创建坐标系信息
geometry_msgs::TransformStamped ts;
//----设置头信息
ts.header.seq = 100;
ts.header.stamp = ros::Time::now();//获取当前时刻
ts.header.frame_id = "base_link";//相对坐标系中被参考的那个
//----设置子级坐标系
ts.child_frame_id = "laser";//雷达的坐标系
//----设置子级相对于父级的偏移量，三个偏移量
ts.transform.translation.x = 0.2;
ts.transform.translation.y = 0.0;
ts.transform.translation.z = 0.5;
//----设置四元数:将 欧拉角数据转换成四元数
tf2::Quaternion qtn;//创建4元素对象
//向该对象设置欧拉角。这个对象可以将欧拉角转换成4元素
qtn.setRPY(0,0,0);//有3个值，就是俯仰，翻滚，偏航值
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();
// 5.广播器发布坐标系信息
broadcaster.sendTransform(ts);
ros::spin();
return 0;
}
使用：
rosmsg info geometry_msgs/TransformStamped
-
header:
seq: 100 序列号
stamp:
secs: 1681440325 时间戳
nsecs: 631626405 时间戳
frame_id: "base_link" 小车底盘
child_frame_id: "laser" 雷达
transform:
translation:
x: 0.2 x，y，z偏移量
y: 0.0
z: 0.5
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
订阅方
/*
静态坐标变换发布方:
发布关于 laser 坐标系的位置信息
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建静态坐标转换广播器
4.创建坐标系信息
5.广播器发布坐标系信息
6.spin()
*/
// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"//
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"static_brocast");
// 3.创建静态坐标转换广播器
tf2_ros::StaticTransformBroadcaster broadcaster;
// 4.创建坐标系信息
geometry_msgs::TransformStamped ts;
//----设置头信息
ts.header.seq = 100;
ts.header.stamp = ros::Time::now();//获取当前时刻
ts.header.frame_id = "base_link";//相对坐标系中被参考的那个
//----设置子级坐标系
ts.child_frame_id = "laser";//雷达的坐标系
//----设置子级相对于父级的偏移量，三个偏移量
ts.transform.translation.x = 0.2;
ts.transform.translation.y = 0.0;
ts.transform.translation.z = 0.5;
//----设置四元数:将 欧拉角数据转换成四元数
tf2::Quaternion qtn;//创建4元素对象
//向该对象设置欧拉角。这个对象可以将欧拉角转换成4元素
qtn.setRPY(0,0,0);//有3个值，就是俯仰，翻滚，偏航值
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();
// 5.广播器发布坐标系信息
broadcaster.sendTransform(ts);
ros::spin();
return 0;
}
使用python版
安装tf
sudo apt-get install ros-noetic-tf
发布方
#! /usr/bin/env python
"""
静态坐标变换发布方:
发布关于 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()
订阅方
#! /usr/bin/env python
"""
订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，
转换成父级坐标系中的坐标点
实现流程:
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()
使用命令方式进行转换
rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
参数分析
rosrun tf2_ros static_transform_publisher
下面3个，比如摄像头对于底盘的偏移量
0.2：x方向
0 ：y方向
0.5：z方向
下面这3个是，偏航角，俯仰角，翻滚角
0 0 0
父集坐标名称：
/baselink
子集坐标名称
/laser
查看节点
rostopic list
结果
/rosout
/rosout_agg
/tf_static
查看节点信息
rostopic echo /tf_static
结果
transforms:
-
header:
seq: 0
stamp:
secs: 1681455248
nsecs: 158705720
frame_id: "/baselink"
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
---
动态坐标变换
查看小乌龟的话题，消息，格式
查看话题
rostopic list
查看消息
rostopic info /turtle1/pose
查看消息的格式
rosmsg info turtlesim/Pose
创建功能包
创建项目功能包依赖于
tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim
发布方：demo01_dynamic_pub.cpp
/*
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 turtlesim_node,//1.包含头文件
#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 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一个坐标点(相对于子级坐标系)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time();
point_laser.point.x = 1;
point_laser.point.y = 1;
point_laser.point.z = 0;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"world");
ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常:%s",e.what());
}
r.sleep();
ros::spinOnce();
}
return 0;
}
该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
控制乌龟运动，将两个坐标系的相对位置动态发布
实现分析:
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
*/
// 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"//转换四元数的头文件
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 5-1.创建 TF 广播器
static tf2_ros::TransformBroadcaster broadcaster;//创建静态的发布者对象，可重复利用
// 5-2.创建 广播的数据(通过 pose 设置)
geometry_msgs::TransformStamped tfs;//创建数据对象
// |----头设置
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();//获取当前时刻
// |----坐标系 ID
tfs.child_frame_id = "turtle1";
// |----坐标系相对信息设置
tfs.transform.translation.x = pose->x;//从pose中获取
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
// |--------- 四元数设置
tf2::Quaternion qtn;
qtn.setdemo01_dynamic_pub.cpp
RPY(0,0,pose->theta);//1.包含头文件
#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 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一个坐标点(相对于子级坐标系)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time();
point_laser.point.x = 1;
point_laser.point.y = 1;
point_laser.point.z = 0;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"world");
ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常:%s",e.what());
}
r.sleep();
ros::spinOnce();
}
return 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-3.广播器发布数据
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(ademo01_dynamic_pub.cpp
rgc,argv,"dynamic_tf_pub");
// 3.创建 ROS 句柄
ros::NodeHandle nh;
// 4.创建订阅对象
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
// 5.回调函数处理订阅到的数据(实现TF广播)
//
// 6.spin
ros::spin();
return 0;
}
订阅方：demo02_dynamic_sub.cpp
//1.包含头文件
#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 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一个坐标点(相对于子级坐标系)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time();
point_laser.point.x = 1;
point_laser.point.y = 1;
point_laser.point.z = 0;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"world");
ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常:%s",e.what());
}
r.sleep();
ros::spinOnce();
}
return 0;
}
创建小乌龟
roscore
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key
实现发布订阅
source ./devel/setup.bash
rosrun tf_doteiass demo01_dynamic_pub
rosrun tf_doteiass demo02_dynamic_sub
打开rviz进行查看
rviz
python实现
发布方 :demo01_dynamic_pub_p.py
#! /usr/bin/env python
"""
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
控制乌龟运动，将两个坐标系的相对位置动态发布
实现分析:
1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3.将 pose 信息转换成 坐标系相对信息并发布
实现流程:
1.导包
2.初始化 ROS 节点
3.订阅 /turtle1/pose 话题消息
4.回调函数处理
4-1.创建 TF 广播器
4-2.创建 广播python实现
demo01_dynamic_pub_p.py
的数据(通过 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
#转换成4元素
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()
订阅方：demo02_dynamic_sub_p.py
#! /usr/bin/env python
"""
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 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()