ROS TF coordinate transformation Library & rewrite to create high frequency coordinate transformation broadcaster

introduction

​ Coordinate transformation is a basic concept in robotics , The motion of each joint in a robot , There are corresponding coordinate transformation relations . Coordinate transformation can be divided into position transformation and attitude transformation , It can be expressed by position vector and rotation matrix respectively , And these two parts can form 4*4 Homogeneous transformation matrix , It can be expressed as follows ：

$$

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}

px&py&p_z

end{matrix}right]$$

​ stay ROS The transformation of coordinate system in is made by the coordinate system transformation system TF Library to implement .

brief introduction

​ TF It is a function package that allows users to track multiple coordinate systems over time , It uses a tree data structure , Buffer and maintain coordinate transformation relations among multiple coordinate systems according to time , Can help developers at any time , Complete points between coordinate systems 、 Transformation of coordinates such as vectors .

​ TF Can operate in a distributed system , That is to say, all coordinate transformation relations in a robot system , Components are available for all nodes , All subscriptions TF The node of the message will buffer a transformation relationship data of all coordinate systems , So this structure does not need a central server to store any data .

TF Library usage steps

1. monitor TF Transformation

Receive and cache all coordinate transformation data published in the system , And query the required coordinate transformation relationship .

1. radio broadcast TF Transformation

Broadcast the coordinate transformation relationship between coordinate systems in the system . There may be many different parts of the system TF Change broadcast , Each broadcast can directly insert the coordinate transformation relationship into TF In the tree , No more synchronization is needed .

tf_broadcaster

​ tf_broadcaster yes TF The realization of broadcasting coordinate transformation information in the library , We can take a look at the broadcast coordinate transformation given on the official website below demo.

#include <ros/ros.h>

#include <tf/transform_broadcaster.h>

#include <turtlesim/Pose.h>

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;

};

To use TransformBroadcaster, We need to include tf/transform_broadcaster.h The header file .

among , The most critical step is sendTransform, Only here can the broadcast of coordinate information be realized .

Rewrite to create a high frequency coordinate transformation broadcaster

​ Because the actual robot development , Implementing complex functions often means that high real-time performance is required , While using ros Self contained tfbroadcaster It may not meet our requirements , So here we plan to rewrite and create a high-frequency coordinate transformation broadcaster , First of all, refer to tf It's in the library tfbroadcaster Implementation code transform_broadcaster.cpp· GitHub.

#include "ros/ros.h"

#include "tf/transform_broadcaster.h"

#include "tf/transform_listener.h"

#include <tf2_ros/transform_broadcaster.h>

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<geometry_msgs::TransformStamped> & msgtf)

{

  tf2_broadcaster_.sendTransform(msgtf);

}

void TransformBroadcaster::sendTransform(const std::vector<StampedTransform> & transforms)

{

  std::vector<geometry_msgs::TransformStamped> msgtfs;

  for (std::vector<StampedTransform>::const_iterator it = transforms.begin(); it != transforms.end(); ++it)

  {

    geometry_msgs::TransformStamped msgtf;

    transformStampedTFToMsg(*it, msgtf);

    msgtfs.push_back(msgtf);

  }

  tf2_broadcaster_.sendTransform(msgtfs);

} 

  

}

​ We can see ,tfbroadcaster The implementation of is not complicated , Just receiving StampedTransform or geometrymsgs:: TransformStamped> Information , And then through tf2_ros:: TransformBroadcaster send out .

​ Rewriting ideas ： Here we just need to rewrite sendTransform function , take tf2ros:: TransformBroadcaster The release message is modified to use realtimetools:: RealtimePublisher msgs::TFMessage> Just publish the information . Because only broadcasting is needed in the project development geometrymsgs:: TransformStamped Information , So only rewrite the related functions .

Realization

 /**

     * @brief  Broadcast send coordinate system conversion message 

     *

     * @param transform  Coordinate conversion messages 

     */

    void realtime_broadcaster::sendTransform(const geometry_msgs::TransformStamped& transform)

    {

        std::vector<geometry_msgs::TransformStamped> TransformStampeds;

        TransformStampeds.push_back(transform);

        sendTransform(TransformStampeds);

    }

    void realtime_broadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped>& 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();

        }

    }

Reference resources

transform_broadcaster.cpp· GitHub

ROS Every bit of Technology —— tf Coordinate transformation library