赞
踩
tf2是一个随时间跟踪多个坐标系的功能包,使用tf2功能包总的来说就只有两步:1、监听TF变换2、广播TF变换你也许会被静态/动态/多坐标转换弄晕了头,但是你只要认清坐标转换的实质就会恍然大悟。无论是哪种坐标转换,其实质就是利用tf2_ros::Buffer类对象中缓存的坐标间的相互关系将坐标点基于X坐标系的坐标转化至基于Y坐标系的坐标。
1.1 多坐标变换
我们以坐标系A、坐标系B、坐标系C、坐标系D三个坐标系相互转化为例进行说明:
准确的来说lookupTransfrom函数的作用是取得树状图中两个从未有联系的坐标系得相对关系:坐标系C与坐标系D的相对关系。可以取得树状图中已经建立联系的坐标系间的相对关系吗?可以,但是没有什么必要,如果我们仅仅使用已知的坐标系关系那么lookupTransfrom函数有无均可。
1.2 静态/动态坐标变换
我们会发现:两者都有的地方就是“调用transfrom函数进行坐标转换”。此外,两者不同之处在于坐标系间的转换关系获取,多坐标系转换中需要调用lookupTransfrom函数对buffer缓冲区内的数据进行复杂计算得到任意两个坐标系的转换关系,而静态/动态坐标系转换关系只需订阅static_tf话题即可得到。
1.3 各功能包作用
我们知道使用tf坐标转换除了需要roscpp、rospy、std_msgs这三个基本的功能包外,还需要tf2、tf2_geometry_msgs、geometry_msgs、tf2_ros这四个额外的功能包,我们需要知道他们分别的作用,这样有助于我们记忆:
tf2功能包:该功能包用于根据坐标系信息以及坐标系间的关系计算得到坐标系之间坐标变换关系(向量/坐标系的旋转)
geometry_msgs功能包:里面包含了常见坐标系的数据类型(eg. TransformStamped数据类型包含了参考坐标系的名称,参考坐标系创建的时间以及子坐标系相较于参考坐标系的相对位置(旋转+平移);PointStamped数据类型包含了子坐标系中点的坐标)
tf2_ros功能包:该功能包用于在节点之间发送信息并进行相关操作,这些都是基于内部的NodeHandle对象实现的(eg. Tf2_ros功能包中StaticTransformBroadcaster 用于发布static_tf话题消息;TransformListener用于订阅static_tf话题消息;Buffer用于缓存订阅节点订阅到的static_tf话题消息)
tf2_geometry_msgs功能包:该功能包的主要作用就是根据“接收到的坐标系相互关系的信息(TransformStamped数据)以及自身子坐标系中坐标点的信息(PointStamped数据)”通过tf2功能包计算得到“参考坐标系下坐标点的位置信息”。此外类似的还有如下的功能包:
这些功能包的功能就是充当数据类型转换的媒介,将其他数据类型转换为tf2坐标变换工具可以识别的数据类型。
注意: tf2_geometry_msgs功能包可以根据tf2功能包得到的坐标变换关系结合被转换坐标系坐标点坐标得出目标坐标系坐标点的坐标,这要与tf2功能包得出的坐标变换关系区分开!
四元数:
彻底搞懂“旋转矩阵/欧拉角/四元数”,让你体会三维旋转之美
旋转矩阵:
坐标变换最通俗易懂的解释(推到+图解)
我们要实现坐标的变换就必须清楚订阅者和发布者所实现的功能,即整体功能的实现逻辑:
我们读完下面的发布者/订阅者的代码实现可能会有以下几点疑惑:
// 创建发布者对象的头文件
#include "tf2_ros/static_transform_broadcaster.h"
// 创建发布者对象
tf2_ros::StaticTransformBroadcaster pub;
// 创建订阅者对象的头文件
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
// 创建订阅者对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
Tf2坐标转换中发布消息是.msg类型的,因此我们无论是编写发布方还是订阅方都须遵循话题通信的实现逻辑。
4.1 发布者实现
(1)首先,我们创建发布对象:
// 头文件
#include "tf2_ros/static_transform_broadcaster.h"
// create publisher
tf2_ros::StaticTransformBroadcaster pub;
(2)在发布者中最重要的就是组织发布的信息,代码实现如下:
// 所需头文件 #include "geometry_msgs/TransformStamped.h" // 组织信息 // organize the data geometry_msgs::TransformStamped data; data.header.frame_id = "base_link";//参考坐标系 data.header.stamp = ros::Time::now(); data.child_frame_id = "laser";// 子坐标系 // 子坐标系相对于参考坐标系在参考坐标系X/Y/Z轴方向平移的距离 data.transform.translation.x = 0.1; data.transform.translation.y = 0.5; data.transform.translation.z = 0.3; // 子坐标系各轴相对于参考坐标系各坐标轴旋转的角度 // 这里用到了坐标变换工具包tf2 需要包含头文件,可放在顶部 #include "tf2/LinearMath/Quaternion.h" tf2::Quaternion qtn; // 调用tf2坐标变换工具 //欧拉角转四元数 qtn.setRPY(30,10,5); data.transform.rotation.w = qtn.getW(); data.transform.rotation.x = qtn.getX(); data.transform.rotation.y = qtn.getY(); data.transform.rotation.z = qtn.getZ();
具体参数及含义如下所示:
注意:我们要做的是将子坐标系到参考坐标系的变换关系传输给订阅static_tf话题的订阅者。
(3)信息的发布
// 不断轮询进行坐标系信息的发布
ros::Rate rate(1);
while(ros::ok()){
pub.sendTransform(data);
rate.sleep();
}
4.2 广播器示例:turtle_tf_broadcaster.cpp
#include "turtlesim/msg/pose.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/transform_broadcaster.h" #include "tf2/LinearMath/Quaternion.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" std::string turtle_name; //节点句柄 rclcpp::Node::SharedPtr node_handle = nullptr; void pose_callback(const turtlesim::msg::Pose::SharedPtr pose) { //tf广播器 static tf2_ros::TransformBroadcaster pose_broadcaster_(node_handle); //广播器消息类型实例化 geometry_msgs::msg::TransformStamped pose_tf; //根据海龟当前位姿,发布对世界坐标系的变换 pose_tf.header.stamp = node_handle->now(); pose_tf.header.frame_id = "world"
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。