5.1 TF坐标变换
作者:互联网
5.1 TF坐标变换
机器人系统上,有多个传感器,如激光雷达、摄像头等,有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标,横向、纵向、高度的距离信息)的,以协助机器人定位障碍物,可以直接将物体相对该传感器的方位信息,等价于物体相对于机器人系统或机器人其它组件的方位信息吗?显示是不行的,这中间需要一个转换过程。更具体描述如下:
场景1:雷达与小车
现有一移动式机器人底盘,在底盘上安装了一雷达,雷达相对于底盘的偏移量已知,现雷达检测到一障碍物信息,获取到坐标分别为(x,y,z),该坐标是以雷达为参考系的,如何将这个坐标转换成以小车为参考系的坐标呢?
场景2:现有一带机械臂的机器人(比如:PR2)需要夹取目标物,当前机器人头部摄像头可以探测到目标物的坐标(x,y,z),不过该坐标是以摄像头为参考系的,而实际操作目标物的是机械臂的夹具,当前我们需要将该坐标转换成相对于机械臂夹具的坐标,这个过程如何实现?
当然,根据我们高中学习的知识,在明确了不同坐标系之间的的相对关系,就可以实现任何坐标点在不同坐标系之间的转换,但是该计算实现是较为常用的,且算法也有点复杂,因此在 ROS 中直接封装了相关的模块: 坐标变换(TF)。
概念
tf:TransForm Frame,坐标变换
坐标系:ROS 中是通过坐标系统开标定物体的,确切的将是通过右手坐标系来标定的。
作用
在 ROS 中用于实现不同坐标系之间的点或向量的转换。
案例
小乌龟跟随案例:如本章引言部分演示。
说明
在ROS中坐标变换最初对应的是tf,不过在 hydro 版本开始, tf 被弃用,迁移到 tf2,后者更为简洁高效,tf2对应的常用功能包有:
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
tf2: 封装了坐标变换的常用消息。
tf2_ros:为tf2提供了roscpp和rospy绑定,封装了坐标变换常用的API。
另请参考:
5.1.1 坐标msg消息
订阅发布模型中数据载体 msg 是一个重要实现,首先需要了解一下,在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped
和geometry_msgs/PointStamped
前者用于传输坐标系相关位置信息,后者用于传输某个坐标系内坐标点的信息。在坐标变换中,频繁的需要使用到坐标系的相对关系以及坐标点信息。
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
四元数用于表示坐标的相对姿态
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
另请参考:
-
http://docs.ros.org/en/api/geometry_msgs/html/msg/TransformStamped.html
-
http://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html
5.1.2 静态坐标变换
所谓静态坐标变换,是指两个坐标系之间的相对位置是固定的。
需求描述:
现有一机器人模型,核心构成包含主体与雷达,各对应一坐标系,坐标系的原点分别位于主体与雷达的物理中心,已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物,在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问,该障碍物相对于主体的坐标是多少?
结果演示:
实现分析:
- 坐标系相对关系,可以通过发布方发布
- 订阅方,订阅到发布的坐标系相对关系,再传入坐标点信息(可以写死),然后借助于 tf 实现坐标变换,并将结果输出
实现流程:C++ 与 Python 实现流程一致
- 新建功能包,添加依赖
- 编写发布方实现
- 编写订阅方实现
- 执行并查看结果
方案A:C++实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.发布方
/* 静态坐标变换发布方: 发布关于 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; qtn.setRPY(0,0,0); 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; }
配置文件此处略。
3.订阅方
/* 订阅坐标系信息,生成一个相对于 子级坐标系的坐标点数据,转换成父级坐标系中的坐标点 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建 TF 订阅节点 4.生成一个坐标点(相对于子级坐标系) 5.转换坐标点(相对于父级坐标系) 6.spin() */ //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,"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 = "laser"; point_laser.header.stamp = ros::Time::now(); point_laser.point.x = 1; point_laser.point.y = 2; point_laser.point.z = 7.3; // 5.转换坐标点(相对于父级坐标系) //新建一个坐标点,用于接收转换结果 //--------------使用 try 语句或休眠,否则可能由于缓存接收延迟而导致坐标转换失败------------------------ 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("程序异常....."); } r.sleep(); ros::spinOnce(); } return 0; }
配置文件此处略。
4.执行
可以使用命令行或launch文件的方式分别启动发布节点与订阅节点,如果程序无异常,控制台将输出,坐标转换后的结果。
方案B:Python实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.发布方
#! /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()
权限设置以及配置文件此处略。
3.订阅方
#! /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()
权限设置以及配置文件此处略。
PS: 在 tf2 的 python 实现中,tf2 已经封装了一些消息类型,不可以使用 geometry_msgs.msg 中的类型
4.执行
可以使用命令行或launch文件的方式分别启动发布节点与订阅节点,如果程序无异常,控制台将输出,坐标转换后的结果。
补充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
也建议使用该种方式直接实现静态坐标系相对信息发布。
补充2:
可以借助于rviz显示坐标系关系,具体操作:
- 新建窗口输入命令:rviz;
- 在启动的 rviz 中设置Fixed Frame 为 base_link;
- 点击左下的 add 按钮,在弹出的窗口中选择 TF 组件,即可显示坐标关系。
另请参考:
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28C%2B%2B%29
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28Python%29
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28Python%29
标签:5.1,msgs,point,坐标,TF,ros,坐标系,tf2 来源: https://www.cnblogs.com/zhjblogs/p/14934715.html