本篇博客是慕课《ROS机器人操作系统入门》的第四篇笔记,慕课网址是这里。本篇笔记主要记录tf
和在C++和Python下的两种实现方式、需要注意的问题以及urdf
。
1.基本知识
(1)Transform
简单来说tf
是ROS中用来处理坐标变换而生的一套工具,是Transform的缩写。tf
也可以理解为一个Topic,这个Topic上的消息(Message)就是就是各坐标系(frame)间的变换关系,这些消息都有对应的发布者(Publisher)进行发布与更新。多个变换和坐标系组成树状的图,叫TF树,如下示意图所示。
树状图中每个节点表示一个坐标系(frame),对应机器人的一个link,节点之间的箭头则代表变换关系。各个节点之间必须是可以连通的,否则会报错。
在ROS里两个frame之间的变换是发布者(Publisher)发布的消息,这个消息的格式是在geometry_msgs/TransformStamped.msg
中定义的,如下图所示。
而tf树的数据结构则定义在tf/tfMessage.msg
(一代)或tf2_msgs/TFMessage.msg
(二代),如下。
利用rostopic info /tf
即可以知道当前的tf
是什么格式。
(2)urdf
urdf
全称Unified Robot Description Format,统一机器人描述格式,指定了一个用于描述机器人结构的规范,以xml
文件格式储存。tf
的维护、可视化、仿真等都会用到它。在urdf文件中有link
和joint
。link
对应tf树的坐标系frame,joint
对应tf树的坐标系间的变换transform。link
和joint
的常用属性如下。
2.需求与描述
现有一个机器人,包含有一个RGB-D相机、一个LiDAR一个云台,云台上有一个相机。建立一个tf树来维护它们彼此之间的坐标变换关系,如下图所示。
trans_broadcaster
节点用于发布消息,trans_listener
节点用于接收消息。
3.Transform的C++实现
C++中提供了很多相关数据类型,如下:
注意tf::StampedTransform
和上面提到的geometry_msgs/TransformStamped.msg
的区别,前者是C++中的一个类,只能用于C++中,而后者则是ROS的消息,只依赖于ROS,与语言无关,都可以使用。
TransformBroadcaster
类及相关成员函数。
TransformListener
类及相关成员函数。
注意获取变换的时间,不能是ros::Time::now()
。因为一个消息从发送到接收会有一定的延迟,如果选择现在的时间作为接收,则永远也收不到之前发送的消息,解决办法是ros::Time(0)
,表示接收最近一次发布的消息。
(1)创建Package
打开RoboWare,在”资源管理器”中找到src
文件夹,右键单击选择”新建ROS包”,输入名称即可创建Package
。创建完成后右键Package
选择”编辑依赖的ROS包列表”,输入roscpp
、tf
、geometry_msgs
(空格隔开)即可添加依赖。
(2)创建trans_broadcaster节点
在RoboWare的资源管理器中找到transform_learn
,右键选择”新建Src文件夹”,在src
文件夹上右键,然后选择”新建CPP源文件”,输入名称为trans_broadcaster.cpp
并回车,然后在弹出的对话框中选择”加入到新的可执行文件中”即可。
(3)编写trans_broadcaster节点代码
对于一个Transform的发布者来说,其对外发布tf消息与之前的Topic类似。主要包含:初始化ROS节点(ros::init()
)、新建NodeHandle(ros::NodeHandle
)、构造TransformBroadcaster
(tf::TransformBroadcaster
)、发布消息(broadcaster.sendTransform()
)、循环执行(ros::spin()
)。具体代码如下:
#include<ros/ros.h>
#include<tf/tf.h> // 引入ROS的TF框架
#include<tf/transform_broadcaster.h> // 用于引入广播者相关API
int main(int argc, char *argv[])
{
// 初始化并指定节点名称
ros::init(argc, argv, "trans_broadcaster");
// 新建句柄
ros::NodeHandle nh;
// 新建广播对象,后续调用它的成员函数实现广播
tf::TransformBroadcaster broadcaster;
// 事先定义好各坐标系之间的变换关系
tf::Transform tf_body_plat;
// setOrigin用于设置变换的平移部分,只支持传入向量形式数据
tf_body_plat.setOrigin(tf::Vector3(0.3,0.6,0.1));
// setRotation用于设置变换的旋转部分,只支持传入四元数形式数据
tf_body_plat.setRotation(tf::createIdentityQuaternion());
tf::Transform tf_body_rgbd;
tf_body_rgbd.setOrigin(tf::Vector3(0.4,0.6,0.2));
tf_body_rgbd.setRotation(tf::createIdentityQuaternion());
tf::Transform tf_body_laser;
tf_body_laser.setOrigin(tf::Vector3(0.8,-0.2,0.3));
tf_body_laser.setRotation(tf::createIdentityQuaternion());
tf::Transform tf_plat_cam;
tf_plat_cam.setOrigin(tf::Vector3(0.1,0.3,-0.2));
tf_plat_cam.setRotation(tf::createIdentityQuaternion());
// 构造Rate对象使循环按指定频率运行
ros::Rate rate(1);
while (nh.ok())
{
// broadcaster发送的是StampedTransform类型对象,因此需要对Transform对象转换
// StampedTransform的构造函数四个参数分别是:
// 第一个参数是构造好的Transform数据
// 第二个参数是这个Transform所对应的时间
// 第三个参数是这个Transform的源坐标系名称(from orign)
// 第四个参数是这个Transform的目标坐标系名称(to target)
tf::StampedTransform stf_body_rgbd = tf::StampedTransform(tf_body_rgbd,ros::Time::now(),"body","rgbd");
tf::StampedTransform stf_body_plat = tf::StampedTransform(tf_body_plat,ros::Time::now(),"body","plat");
tf::StampedTransform stf_body_laser = tf::StampedTransform(tf_body_laser,ros::Time::now(),"body","laser");
tf::StampedTransform stf_plat_cam = tf::StampedTransform(tf_plat_cam,ros::Time::now(),"plat","cam");
// 调用成员函数发送广播
broadcaster.sendTransform(stf_body_rgbd);
broadcaster.sendTransform(stf_body_plat);
broadcaster.sendTransform(stf_body_laser);
broadcaster.sendTransform(stf_plat_cam);
// 输出部分提示信息
std::cout<<"broadcasting transform......"<<std::endl;
// 休眠等待
rate.sleep();
}
return 0;
}
(4)创建trans_listener节点
在RoboWare的资源管理器中找到src
文件夹,点击右键,然后选择”新建CPP源文件”,输入名称为trans_listener.cpp
并回车,然后在弹出的对话框中选择”加入到新的可执行文件中”即可。
(5)编写trans_listener节点代码
对于一个Transform的接收者来说,其接收tf消息与之前的Topic类似。主要包含:初始化ROS节点(ros::init()
)、新建NodeHandle(ros::NodeHandle
)、构造TransformListener
(tf::TransformListener
)、接收消息or应用变换(listener.lookupTransform()
orlistener.transformPoint
)。具体代码如下:
#include<ros/ros.h>
#include<tf/tf.h> // 引入ROS的TF框架
#include<tf/transform_listener.h> // 用于引入接收者相关API
#include<geometry_msgs/PointStamped.h> // 代码中用到ROS中定义好的一些几何类型,如PointStamped
int main(int argc, char *argv[])
{
// 初始化节点并指定名称
ros::init(argc, argv, "trans_listener");
// 新建句柄
ros::NodeHandle nh;
// 创建一个tf接收对象,后续调用该对象的成员函数进行接收
tf::TransformListener listener;
// 阻塞等待,直到指定的某两个frame连通,注意参数
// 第一个参数是变换的目标坐标系(to)
// 第二个参数是变换的源坐标系(from)
// 第三个参数指定那一时刻的变换,如果写0则表示当前时刻
// 第四个参数是阻塞时间长度
listener.waitForTransform("body","cam",ros::Time(0),ros::Duration(4.0));
// 新建测试点用于进行测试
geometry_msgs::PointStamped point_in_rgbd;
point_in_rgbd.header.frame_id = "rgbd";
point_in_rgbd.header.stamp = ros::Time();
point_in_rgbd.point.x = 0.4;
point_in_rgbd.point.y = 0.5;
point_in_rgbd.point.z = 0.3;
// 构造Rate对象使循环按指定频率运行
ros::Rate rate(1);
while (nh.ok())
{
// 在正常运行的情况下利用try-catch语句尝试接收
try
{
// 测试一,利用lookupTransform接收
// 新建标记变换对象用于接收收到的变换
tf::StampedTransform trans_body_laser;
// 根据坐标名字查找变换,若找到赋值给传入的参数
// 第一个参数是变换的目标坐标系(to)
// 第二个参数是变换的源坐标系(from)
// 第三个参数指定那一时刻的变换,如果写0则表示当前时刻。ros::Time(0)表示最近的一帧坐标变换,不能写成ros::Time::now()
// 第四个参数表示用于接收变换数据的对象
listener.lookupTransform("laser","body",ros::Time(0),trans_body_laser);
// 获取变换的平移部分
std::cout<<"translation body->laser:x="<<
trans_body_laser.getOrigin().x()<<
",y="<<trans_body_laser.getOrigin().y()<<
",z="<<trans_body_laser.getOrigin().z()<<std::endl<<std::endl;
// 测试二,利用transformPoint应用变换
// 将rgbd下的点转换到cam下
geometry_msgs::PointStamped tf_p_cam;
listener.transformPoint("cam",point_in_rgbd,tf_p_cam);
ROS_INFO("trans1:rgbd_point:(%f,%f,%f) -> cam_point(%f,%f,%f)",
point_in_rgbd.point.x,point_in_rgbd.point.y,point_in_rgbd.point.z,
tf_p_cam.point.x,tf_p_cam.point.y,tf_p_cam.point.z);
// 再将刚刚转换的点转回rgbd下,以此验证是否转换正确闭合
geometry_msgs::PointStamped tf_p_rgbd;
listener.transformPoint("rgbd",tf_p_cam,tf_p_rgbd);
ROS_INFO("trans2:cam_point:(%f,%f,%f) -> rgbd_point(%f,%f,%f)\n",
tf_p_cam.point.x,tf_p_cam.point.y,tf_p_cam.point.z,
tf_p_rgbd.point.x,tf_p_rgbd.point.y,tf_p_rgbd.point.z);
}
catch (tf::TransformException &ex)
{
// 如果失败输出错误信息
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
rate.sleep();
}
return 0;
}
(6)编译运行测试
最后在Catkin工作空间根目录下打开终端,使用catkin_make
进行编译。然后先输入roscore
打开Master
,然后在新终端中输入rosrun transform_learn trans_broadcaster
启动trans_broadcaster
节点,新终端中输入rosrun transform_learn trans_listener
启动trans_listener
节点。如下:
可以看到成功运行了刚刚写的两个节点,并且实现了tf消息的收发。最后简单说一下,生成的两个节点的可执行文件放在了/root/catkin_ws/devel/lib/transform_learn
里,直接在终端中运行这个可执行文件也是可以的(Master
已经运行的情况下)。
4.Topic的Python实现
TF相关数据类型:
tf.transformations
包提供了一些基本的变换运算。
tf.TransformListener
类提供了接收者的相关函数。
tf.TransformBroadcaster
类提供了发布者的相关函数。
(1)创建Package
打开RoboWare,在”资源管理器”中找到src
文件夹,右键单击选择”新建ROS包”,输入名称即可创建Package
。创建完成后右键Package
选择”编辑依赖的ROS包列表”,输入rospy
、tf
、geometry_msgs
(空格隔开)即可添加依赖。
(2)创建trans_broadcaster节点文件
根据之前的习惯,src
文件夹里一般放C/C++文件,Python代码一般放在scripts
文件夹里。在包名上右键点击,选择”新建文件夹”,输入名称为scripts
,在scripts
文件夹上右键选择”新建文件”,输入trans_broadcaster.py
即可。
(3)编写trans_broadcaster代码
在Python中有两种发布方法:sendTransform()
和sendTransformMessage()
。第一种Python代码如下:
# coding=utf-8
import rospy
import tf
if __name__ == "__main__":
# 初始化并指定节点名称
rospy.init_node("trans_broadcaster")
# 新建广播对象,后续调用它的成员函数实现广播
broadcaster = tf.TransformBroadcaster()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 在Python中向量、点、四元数、矩阵都表示成类似数组形式,Tuple、List、Numpy Array通用
# 调用成员函数发送广播
# 第一个参数是以tuple表示的平移分量
# 第二个参数是以tuple表示的旋转分量,四元数顺序是(x, y, z, w)
# 第三个参数是变换对应的时间
# 第四个参数是变换的目标坐标系(to target)
# 第五个参数是变换的源坐标系(from origin)
broadcaster.sendTransform((0.4,0.6,0.2),(0,0,0,1),rospy.Time.now(),"rgbd","body")
broadcaster.sendTransform((0.3,0.6,0.1),(0,0,0,1),rospy.Time.now(),"plat","body")
broadcaster.sendTransform((0.8,-0.2,0.3),(0,0,0,1),rospy.Time.now(),"laser","body")
broadcaster.sendTransform((0.1,0.3,-0.2),(0,0,0,1),rospy.Time.now(),"cam","plat")
print "broadcasting transform..."
rate.sleep()
第二种如下:
# coding=utf-8
import rospy
import tf
import geometry_msgs.msg
import tf2_ros.transform_broadcaster
if __name__ == "__main__":
# 初始化并指定节点名称
rospy.init_node("trans_broadcaster")
# 新建广播对象,后续调用它的成员函数实现广播
broadcaster = tf.TransformBroadcaster()
# 相比于第一种方法,先构建好Message然后再广播,这样写起来好看一些
tf_body_plat = geometry_msgs.msg.TransformStamped()
tf_body_plat.header.frame_id = "body"
tf_body_plat.header.stamp = rospy.Time(0)
tf_body_plat.child_frame_id = "plat"
tf_body_plat.transform.translation.x = 0.3
tf_body_plat.transform.translation.y = 0.6
tf_body_plat.transform.translation.z = 0.1
tf_body_plat.transform.rotation.w = 1
tf_body_plat.transform.rotation.x = 0
tf_body_plat.transform.rotation.y = 0
tf_body_plat.transform.rotation.z = 0
tf_body_rgbd = geometry_msgs.msg.TransformStamped()
tf_body_rgbd.header.frame_id = "body"
tf_body_rgbd.header.stamp = rospy.Time(0)
tf_body_rgbd.child_frame_id = "rgbd"
tf_body_rgbd.transform.translation.x = 0.4
tf_body_rgbd.transform.translation.y = 0.6
tf_body_rgbd.transform.translation.z = 0.2
tf_body_rgbd.transform.rotation.w = 1
tf_body_rgbd.transform.rotation.x = 0
tf_body_rgbd.transform.rotation.y = 0
tf_body_rgbd.transform.rotation.z = 0
tf_body_laser = geometry_msgs.msg.TransformStamped()
tf_body_laser.header.frame_id = "body"
tf_body_laser.header.stamp = rospy.Time(0)
tf_body_laser.child_frame_id = "laser"
tf_body_laser.transform.translation.x = 0.8
tf_body_laser.transform.translation.y = -0.2
tf_body_laser.transform.translation.z = 0.3
tf_body_laser.transform.rotation.w = 1
tf_body_laser.transform.rotation.x = 0
tf_body_laser.transform.rotation.y = 0
tf_body_laser.transform.rotation.z = 0
tf_plat_cam = geometry_msgs.msg.TransformStamped()
tf_plat_cam.header.frame_id = "plat"
tf_plat_cam.header.stamp = rospy.Time(0)
tf_plat_cam.child_frame_id = "cam"
tf_plat_cam.transform.translation.x = 0.1
tf_plat_cam.transform.translation.y = 0.3
tf_plat_cam.transform.translation.z = -0.2
tf_plat_cam.transform.rotation.w = 1
tf_plat_cam.transform.rotation.x = 0
tf_plat_cam.transform.rotation.y = 0
tf_plat_cam.transform.rotation.z = 0
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 调用成员函数发送广播
broadcaster.sendTransformMessage(tf_body_plat)
broadcaster.sendTransformMessage(tf_body_laser)
broadcaster.sendTransformMessage(tf_body_rgbd)
broadcaster.sendTransformMessage(tf_plat_cam)
print "broadcasting transform..."
rate.sleep()
(4)创建trans_listener节点文件
在scripts
文件夹上右键选择”新建文件”,输入trans_listener.py
即可。
(5)编写trans_listener代码
代码如下:
# coding=utf-8
import rospy
import tf
import geometry_msgs.msg
if __name__ == "__main__":
# 初始化节点并指定名称
rospy.init_node("trans_listener")
# 创建一个tf接收对象,后续调用该对象的成员函数进行接收
listener = tf.TransformListener()
# 阻塞直到frame相通
listener.waitForTransform("body", "cam", rospy.Time(), rospy.Duration(4.0))
# 新建测试点用于进行测试
point_in_rgbd = geometry_msgs.msg.PointStamped()
point_in_rgbd.header.frame_id = "rgbd"
point_in_rgbd.header.stamp = rospy.Time()
point_in_rgbd.point.x = 0.4
point_in_rgbd.point.y = 0.5
point_in_rgbd.point.z = 0.3
# 构造Rate对象使循环按指定频率运行
rate = rospy.Rate(1)
while not rospy.is_shutdown():
try:
# 测试一,利用lookupTransform接收,监听对应的tf,返回平移和旋转
# ros::Time(0)表示最近的一帧坐标变换,不能写成ros::Time::now()
(translation, rotation) = listener.lookupTransform(
"laser", "body", rospy.Time(0))
print "translation body->laser:", translation, "\n"
# 测试二,利用transformPoint应用变换
# 将rgbd下的点转换到cam下
tf_p_cam = listener.transformPoint("cam", point_in_rgbd)
rospy.loginfo("trans1:rgbd_point:(%f,%f,%f) -> cam_point(%f,%f,%f)",
point_in_rgbd.point.x, point_in_rgbd.point.y, point_in_rgbd.point.z,
tf_p_cam.point.x, tf_p_cam.point.y, tf_p_cam.point.z)
# 再将刚刚转换的点转回rgbd下,以此验证是否转换正确闭合
tf_p_rgbd = listener.transformPoint("rgbd", tf_p_cam)
rospy.loginfo("trans2:cam_point:(%f,%f,%f) -> rgbd_point(%f,%f,%f)\n",
tf_p_cam.point.x, tf_p_cam.point.y, tf_p_cam.point.z,
tf_p_rgbd.point.x, tf_p_rgbd.point.y, tf_p_rgbd.point.z)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
rate.sleep()
(6)运行测试
最后在终端中输入roscore
启动Master
,在scripts
目录下打开新终端然后分别输入python trans_broadcaster1.py
启动广播节点,python trans_listener.py
启动接收节点。运行结果如下:
最后所有代码上传到了Github,点击查看。
5.tf常用工具介绍
rosrun tf view_frames
:订阅tf
树5秒钟,然后保存成一个pdf文件。例如本文开头的树状图就是用这个工具生成的。
rosrun rqt_tf_tree rqt_tf_tree
:动态查看当前的tf
树。
rosrun tf tf_echo [reference_frame] [target_frame]
:查看指定两个frame之间的变换关系。
本文作者原创,未经许可不得转载,谢谢配合