ROS TF坐标变换库 & 重写创建高频坐标变换广播器
文章目录
-
- 引言
- 简介
- TF库使用步骤
- tf_broadcaster
- 重写创建高频坐标变换广播器
-
- 实现
- 参考
- 后续
引言
坐标变换是机器人学中的基础概念，机器人中每个关节的运动，都存在相应的坐标变换关系。坐标变换可以分为位置变换和姿态变换，分别可由位置矢量和旋转矩阵表示，而这两部分可以组成4*4的齐次变换矩阵，可表示如下：
T = [ R P 0 1 ] R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] P = [ p x p y p z ] T=\left[\begin{matrix} R & P \\ 0&1 \end{matrix}\right]\\ R=\left[\begin{matrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33} \end{matrix}\right] P=\left[\begin{matrix} p_x&p_y&p_z \end{matrix}\right] T=[R0P1]R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤ P=[pxpypz]
在ROS中坐标系变换由坐标系变换系统TF库来实现。
简介
TF是一个让用户随时间跟踪多个坐标系的功能包，它使用树型数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间，在坐标系间完成点、向量等坐标的变换。
TF可以在分布式系统中进行操作，也就是说一个机器人系统中所有的坐标变换关系，对于所有节点组件都是可用的，所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据。
TF库使用步骤
- 监听TF变换
接收并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系。 - 广播TF变换
向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入TF树中，不需要再进行同步。
tf_broadcaster
tf_broadcaster是TF库中广播坐标变换信息的实现，我们可以看看下面官网给出的广播坐标变换的demo。
#include
#include
#include
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
turtle_name = argv[1];
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
要使用TransformBroadcaster，我们需要包含tf/transform_broadcaster.h头文件。
其中，最关键的一步为sendTransform，在这里才实现了坐标信息的广播。
重写创建高频坐标变换广播器
由于实际的机器人开发中，实现复杂的功能往往意味着需要满足较高的实时性，而使用ros自带的tf_broadcaster不一定能够满足我们的要求，所以这里计划重写创建一个高频坐标变换广播器，首先要参考tf库中自带tf_broadcaster的实现代码transform_broadcaster.cpp· GitHub。
#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include
namespace tf {
TransformBroadcaster::TransformBroadcaster():
tf2_broadcaster_()
{
}
void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
{
tf2_broadcaster_.sendTransform(msgtf);
}
void TransformBroadcaster::sendTransform(const StampedTransform & transform)
{
geometry_msgs::TransformStamped msgtf;
transformStampedTFToMsg(transform, msgtf);
tf2_broadcaster_.sendTransform(msgtf);
}
void TransformBroadcaster::sendTransform(const std::vector & msgtf)
{
tf2_broadcaster_.sendTransform(msgtf);
}
void TransformBroadcaster::sendTransform(const std::vector & transforms)
{
std::vector msgtfs;
for (std::vector::const_iterator it = transforms.begin(); it != transforms.end(); ++it)
{
geometry_msgs::TransformStamped msgtf;
transformStampedTFToMsg(*it, msgtf);
msgtfs.push_back(msgtf);
}
tf2_broadcaster_.sendTransform(msgtfs);
}
}
我们可以看见，tf_broadcaster的实现并不复杂，仅仅只是接收StampedTransform或geometry_msgs::TransformStamped>信息，然后通过tf2_ros::TransformBroadcaster发送。
重写思路：这里我们只需要重写sendTransform函数，将tf2_ros::TransformBroadcaster发布消息修改成用realtime_tools::RealtimePublisher
实现
/**
* @brief 广播发送坐标系转换消息
*
* @param transform 坐标转换消息
*/
void realtime_broadcaster::sendTransform(const geometry_msgs::TransformStamped& transform)
{
std::vector TransformStampeds;
TransformStampeds.push_back(transform);
sendTransform(TransformStampeds);
}
void realtime_broadcaster::sendTransform(const std::vector& transforms)
{
tf2_msgs::TFMessage message;
for (const auto& transform : transforms)
{
message.transforms.push_back(transform);
}
if (realtime_pub_->trylock())
{
realtime_pub_->msg_ = message;
realtime_pub_->unlockAndPublish();
}
}
参考
transform_broadcaster.cpp· GitHub
ROS技术点滴 —— tf坐标变换库
后续
喜欢的话可以关注一下我的公众号技术开发小圈，尤其是对深度学习以及计算机视觉有兴趣的朋友，我会把相关的源码以及更多资料发在上面，希望可以帮助到新入门的大家！
