Wiki

Coordinate transformation TF

Updated 2023-12-30About 56 min read

Existing English translations were reused. 12 current source blocks remain in Simplified Chinese, without triggering paid translation.

1.Coordinate transformation

1.1.Introduction and Application Scenarios

里程计ODOM

惯性计IMU

激光雷达Laser

摄像头Camera

Scenario 1: There is a mobile robot chassis, and a radar is installed on the chassis. The offset of the radar relative to the chassis is known. Now the radar detects an obstacle and obtains coordinates (x, y, z), which are in the radar reference frame. How can these coordinates be converted to coordinates in the robot reference frame?

The lateral and longitudinal distances between the LiDAR and the center or edge of the vehicle, as well as the distance between the LiDAR and the wall and the distance between the vehicle and the wall.

Scenario 2: There is a robot with a robotic arm (e.g., PR2) that needs to grasp a target object. The robot's head camera can currently detect the coordinates (x, y, z) of the target object, but these coordinates are in the camera's reference frame. However, the actual operation of the target object is performed by the gripper of the robotic arm. Currently, we need to transform these coordinates into coordinates relative to the robotic arm's gripper. How is this process implemented?

The above can be calculated using TF.

1.2.Concept and Role

TF implementationRight-handed coordinate system

What matters is relative position and time: at a certain time, a certain object is at a certain position. (If the time difference is too large, the data will be discarded.)

1.3.Case installation and operation

Installation example for Turtle:


# Humble版本安装
sudo apt-get install ros-humble-turtle-tf2-py ros-humble-tf2-tools ros-humble-tf-transformations

# Jazzy版本安装
sudo apt-get install ros-jazzy-turtle-tf2-py ros-jazzy-tf2-tools ros-jazzy-tf-transformations

Additionally, a Python package named transforms3d needs to be installed, which provides quaternion and Euler angle transformation functionality for the tf_transformations package. The installation command is as follows:


# 方式一（不推荐）
sudo apt intall python3-pip
pip3 install transforms3d

#方式二（推荐）
sudo apt install python3-transforms3d

Open two terminals. In terminal 1, enter the following command:

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

This command launches the turtlesim_node node. The turtlesim_node node comes with a small turtle, turtle1, and additionally generates a new turtle, turtle2. turtle2 will move to the position of turtle1.

Terminal 2 enters the following command:

ros2 run turtlesim turtle_teleop_key

In this terminal, you can control the movement of turtle1 via the keyboard, and turtle2 will follow the movement of turtle1 (refer to the section Introduction Case 1 ).

Simp🐢🚹 will follow the turtle🐢 in front and move

1.4.Coordinate transformation related messages

The implementation of coordinate transformation is essentially based on the publish-subscribe model of topic communication. The publisher can publish the relative relationships between coordinate frames, and the subscriber can listen to these messages and perform transformations between different coordinate frames. Obviously, as introduced earlier, in topic communication, interface messages, as the data carrier, are a relatively important part of the entire communication model. This section will introduce two commonly used interface messages in coordinate transformation:geometry_msgs/msg/TransformStampedandgeometry_msgs/msg/PointStamped.

The former is an interface used to describe the relative relationship between two coordinate systems at a certain moment, while the latter is an interface used to describe the position of a coordinate point within a coordinate system at a certain moment. In coordinate transformation, the relative relationship of coordinate systems and coordinate point information are frequently used.

1.4.1.geometry_msgs/msg/TransformStamped

View the interface definition with the following command:

ros2 interface show geometry_msgs/msg/TransformStamped

Interface definition explanation:

std_msgs/Header header
    builtin_interfaces/Time stamp     # 时间戳
        int32 sec         #秒
        uint32 nanosec    #纳秒
    string frame_id                   # 父级坐标系
string child_frame_id                 # 子级坐标系
Transform transform                   # 子级坐标系相对于父级坐标系的位姿
    Vector3 translation               # 三维偏移量
        float64 x
        float64 y
        float64 z
    Quaternion rotation               # 四元数
        float64 x 0
        float64 y 0
        float64 z 0
        float64 w 1

Generally, describing the motion of an object has 6 degrees of freedom: X, Y, Z, Yaw, Pitch, Roll.

Three translations, three rotations:

Vector3 translation represents 3 translations

Quaternion rotation A quaternion can be converted into three Euler angles (yaw, pitch, roll).

(Q: Why use quaternions instead of Euler angles? A: Because calculations using Euler angles suffer from gimbal lock, quaternions are chosen instead of Euler angles in order to avoid the shortcomings of Euler angles.)

3 translations in meters

The three rotations are in radians (rad)

Quaternions are similar to Euler angles in representing the relative orientation of coordinate systems,

具体转化详见大疆开发板C型嵌入式软件教程文档.pdf的18.3节

Specific conversion algorithm (Mahony algorithm) (the TF2 library of ROS2 also has a specific conversion algorithm):

https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

In a right-handed coordinate system, N2 is translated by 1 m relative to N1 along the X-axis (one grid cell represents 1 meter).

Rotation

1.4.2.geometry_msgs/msg/PointStamped

View the interface definition with the following command:

ros2 interface show geometry_msgs/msg/PointStamped

Interface definition explanation:

std_msgs/Header header
    builtin_interfaces/Time stamp    # 时间戳
        int32 sec   #秒
        uint32 nanosec   #纳秒
    string frame_id                  # 参考系
Point point                          # 三维坐标
    float64 x
    float64 y
    float64 z

Coordinate points in three-dimensional space

2.Coordinate transformation broadcasting

2.1.Introduction, Case, and Analysis

There are mainly two types of coordinate system relative relationships: static coordinate system relative relationship and dynamic coordinate system relative relationship.

The so-called static coordinate system relative relationship means that the relative position between two coordinate systems is fixed and unchanging. For example, components such as radar and cameras on a vehicle are generally fixed, so the radar coordinate system relative to the vehicle chassis coordinate system or the camera coordinate system relative to the vehicle chassis coordinate system is a static relationship.

The so-called dynamic coordinate system relative relationship refers to the relative positional relationship between two coordinate systems that changes dynamically. For example, the joints or grippers of a robotic arm on a vehicle, different vehicles in a multi-vehicle formation, etc., are all capable of motion. Therefore, the relative relationship between the coordinate system of a joint or gripper of a robotic arm and the vehicle chassis coordinate system or the coordinate systems of different vehicles is a dynamic relationship.

This section mainly introduces how to implement static coordinate transformation broadcasting and dynamic coordinate transformation broadcasting. In addition, this section also demonstrates how to publish coordinate point messages.

The positions of the LiDAR Laser and the color camera relative to the chassis are static, whereas the position of the robotic arm's end effector relative to the robot is dynamic.

2.1.1.1. Case Requirements

Case 1: There is an unmanned vehicle equipped with a fixed radar and camera on its chassis. It is known that the vehicle chassis, radar, and camera each correspond to a coordinate system, with the origin of each coordinate system taken at its geometric center. It is also known that the three-dimensional translations of the radar coordinate system relative to the chassis coordinate system are: 0.4 meters in the x direction, 0 meters in the y direction, and 0.2 meters in the z direction, with no rotation. The three-dimensional translations of the camera coordinate system relative to the chassis coordinate system are: -0.5 meters in the x direction, 0 meters in the y direction, and 0.4 meters in the z direction, with no rotation. Please broadcast the coordinate system relationship between the radar and the chassis, and the coordinate system relationship between the camera and the chassis, and view the broadcast results in rviz2.

Case 2: Start turtlesim_node, assuming that in this node the window has a world coordinate system (with the lower-left corner as the origin of the coordinate system), and the turtle is another coordinate system; the turtle can be controlled to move via the keyboard. Please dynamically publish the relative relationship between the turtle coordinate system and the world coordinate system.

2.1.2.2. Case Analysis

In the above cases, Case 1 requires static coordinate transformation, while Case 2 requires dynamic coordinate transformation. Regardless of the implementation, there are two key elements of concern:

How to broadcast the relative relationship of coordinate frames;
How to use rviz2 to display the relative relationship of coordinate frames.

2.1.3.3. Process Overview

The process for implementing static or dynamic coordinate transformations using coding is similar, and the main steps are as follows:

Write the broadcaster implementation;
Edit the configuration file;
Compile;
Execute;
View the coordinate system relationships in rviz2.

For the example, we will implement it using C++ and Python respectively, and both will follow the implementation process described above.

Additionally: It should be noted that, besides being implemented through coding, static broadcasters also have built-in related tools in tf2. Without coding, you can directly run the node and pass in parameters representing the relative relationship of coordinate frames to publish a static coordinate frame relationship (that is, the command line; if a command line is available, prefer the command line). Dynamic broadcasters do not provide a similar tool. (That is, you must write code.)

2.1.4.4. Preparatory Work

In the terminal, enter the src directory of the workspace, and call the following two commands to create the C++ package respectively.

ros2 pkg create cpp03_tf_broadcaster --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim

The tf2 function package contains conversion algorithms between quaternions and Euler angles.

tf2_ros功能包内包含了广播对象。

geometry_msgs功能包消息载体

The turtlesim package is for obtaining the turtle 🐢 pose

2.2.静态广播器_命令行实现

2.2.1.1. Static Broadcaster Tool

In the tf2_ros package, an executable file named static_transform_publisher is provided, through which static coordinate system relationships can be broadcast directly. Its usage syntax is as follows.

Format 1:

使用以米为单位的 x/y/z 偏移量和以弧度为单位的roll/pitch/yaw（可直译为滚动/俯仰/偏航，分别指的是围绕 x/y/z 轴的旋转）向 tf2 发布静态坐标变换。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

The parent coordinate system and the child coordinate system must be specified.

If these optional parameters are not selected, by default the parent coordinate system and the child coordinate system coincide, meaning both the offset and the rotation are 0.

Offset: X, Y, Z

Rotation: QX,QY,QZ,QW or ROLL,PITCH,YAW (in radians)

Timestamp: No need to set; it will start from the publish time.

Format 2:

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and a qx/qy/qz/qw quaternion.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

Note: In the above two formats, except for the--frame-idused to represent the parent coordinate system and the--child-frame-idused to represent the child coordinate system, all other parameters are optional; if a particular option is not specified, the default value will be used directly.

2.2.2.2. Static Broadcaster Tool Usage

Open two terminals. In terminal 1, enter the following command to publish the static coordinate transformation (coincident) of the radar (laser) relative to the chassis (base_link):

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser

base_link is the parent reference frame

laser is a child reference frame

Generally, the paternal reference frame is selected.

2.2.3.3. Viewing coordinate frame relationships with rviz2

Open a new terminal and use the commandrviz2to launch rviz2 and configure the relevant plugins to view coordinate transformation messages:

Set Fixed Frame in Global Options to base_link;
Click the add button to add the TF plugin;
Check show names in the TF plugin.

The coordinate transformation relationship will be displayed graphically in the Grid on the right.

As shown in the figure, the two reference frames coincide.

Open two terminals. In terminal 1, enter the following command to publish the static coordinate transformation of the radar (laser) relative to the chassis (base_link):

ros2 run tf2_ros static_transform_publisher --x 0.4 --y 0 --z 0.2 --yaw 0.5 --roll 0 --pitch 0 --frame-id base_link --child-frame-id laser

Terminal 2 enters the following command to publish the static coordinate transformation of the camera relative to the chassis (base_link):

ros2 run tf2_ros static_transform_publisher --x -0.5 --y 0 --z 0.4 --yaw 0 --roll 0 --pitch 0 --frame-id base_link --child-frame-id camera

2.3.静态广播器_C++实现

2.3.1.Framework setup

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

add_executable(demo01_static_tf_broadcaster src/demo01_static_tf_broadcaster.cpp)
ament_target_dependencies(
  demo01_static_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

install(TARGETS demo01_static_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME})

2.3.2.Broadcast implementation

/*  
  需求：编写静态坐标变换程序，执行时传入两个坐标系的相对位姿关系以及父子级坐标系id，
       程序运行发布静态坐标变换。
  步骤：
    1.包含头文件；
    2.判断终端传入的参数是否合法；
    3.初始化 ROS 客户端；
    4.定义节点类；
      4-1.创建静态坐标变换发布方；
      4-2.组织并发布消息。
    5.调用 spin 函数，并传入对象指针；
    6.释放资源。

*/

// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

using std::placeholders::_1;

// 4.定义节点类；
class MinimalStaticFrameBroadcaster : public rclcpp::Node
{
public:
  explicit MinimalStaticFrameBroadcaster(char * transformation[]): Node("minimal_static_frame_broadcaster")
  {
    // 4-1.创建静态坐标变换发布方；
    tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    this->make_transforms(transformation);
  }

private:
  // 4-2.组织并发布消息。
  void make_transforms(char * transformation[])
  {
    // 组织消息
    geometry_msgs::msg::TransformStamped t;

    rclcpp::Time now = this->get_clock()->now();
    t.header.stamp = now;
    t.header.frame_id = transformation[7];
    t.child_frame_id = transformation[8];

    t.transform.translation.x = atof(transformation[1]);
    t.transform.translation.y = atof(transformation[2]);
    t.transform.translation.z = atof(transformation[3]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[4]),
      atof(transformation[5]),
      atof(transformation[6]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // 发布消息
    tf_publisher_->sendTransform(t);
  }
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};

int main(int argc, char * argv[])
{
  // 2.判断终端传入的参数是否合法；
  auto logger = rclcpp::get_logger("logger");

  if (argc != 9) {
    RCLCPP_INFO(
      logger, "运行程序时请按照：x y z roll pitch yaw frame_id child_frame_id 的格式传入参数");
    return 1;
  }

  // 3.初始化 ROS 客户端；
  rclcpp::init(argc, argv);
  // 5.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<MinimalStaticFrameBroadcaster>(argv));
  // 6.释放资源。
  rclcpp::shutdown();
  return 0;
}

Parameters must be passed correctly; otherwise, an error will be thrown.

Function to create an organization and publish data

Use the simplest overload.

This way it can be sent. Next, edit the content to be sent.

Set the stamp timestamp to the current time; the now() function sets it to the current time.

atof() converts to the float floating-point type

x() and getx() both retrieve the quaternion x. (y, z, w follow the same pattern.)

In the terminal, enter the current workspace and compile the package:

colcon build --packages-select cpp03_tf_broadcaster

In the current workspace, start two terminals. In terminal 1, enter the following command to publish the static coordinate transformation of the radar (laser) relative to the chassis (base_link):

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser

Reference static broadcaster (command) content to start and configure rviz2. The final execution result is similar to Case 1.

Essentially, it is topic communication, but what is the topic of this topic communication?

By examining the source code of this class, it can be known that the topic is /tf_static

There are two publishers and one subscriber.

These two are the publishers

This is the subscriber

2.4.动态广播器_C++实现

2.4.1.Framework setup

CMakeLists.txt The file needs the following content added:

add_executable(demo02_dynamic_tf_broadcaster src/demo02_dynamic_tf_broadcaster.cpp)
ament_target_dependencies(
  demo02_dynamic_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

Change the content of install in the file to the following:

install(TARGETS demo01_static_tf_broadcaster
  demo02_dynamic_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME})

2.4.2.Broadcast implementation

/*   
  需求：编写动态坐标变换程序，启动 turtlesim_node 以及 turtle_teleop_key 后，该程序可以发布
       乌龟坐标系到窗口坐标系的坐标变换，并且键盘控制乌龟运动时，乌龟坐标系与窗口坐标系的相对关系
       也会实时更新。

  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建动态坐标变换发布方；
      3-2.创建乌龟位姿订阅方；
      3-3.根据订阅到的乌龟位姿生成坐标帧并广播。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/
// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

using std::placeholders::_1;

// 3.定义节点类；
class MinimalDynamicFrameBroadcaster : public rclcpp::Node
{
public:
  MinimalDynamicFrameBroadcaster(): Node("minimal_dynamic_frame_broadcaster")
  {
    // 3-1.创建动态坐标变换发布方；
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    std::string topic_name = "/turtle1/pose";

    // 3-2.创建乌龟位姿订阅方；
    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&MinimalDynamicFrameBroadcaster::handle_turtle_pose, this, _1));
  }

private:
  // 3-3.根据订阅到的乌龟位姿生成坐标帧并广播。   
  void handle_turtle_pose(const turtlesim::msg::Pose & msg)
  {
    // 组织消息
    geometry_msgs::msg::TransformStamped t;
    rclcpp::Time now = this->get_clock()->now();

    t.header.stamp = now;
    t.header.frame_id = "world";   //窗体坐标系
    t.child_frame_id = "turtle1";  //乌龟坐标系

    t.transform.translation.x = msg.x;
    t.transform.translation.y = msg.y;
    t.transform.translation.z = 0.0;      //乌龟在平面内运动

    //从欧拉角转换为四元数
    tf2::Quaternion q;
    q.setRPY(0, 0, msg.theta);        //乌龟只有Yaw
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();
    // 发布消息
    tf_broadcaster_->sendTransform(t);
  }
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
  // 2.初始化 ROS 客户端；
  rclcpp::init(argc, argv);
  // 4.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<MinimalDynamicFrameBroadcaster>());
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

Input parameter:

Parameter 1 topic name

Parameter 2 QoS

Parameter 3 callback function

When the turtle is controlled to move, it will also move in Rviz2.

2.5.坐标点发布_C++实现

2.5.1.Case and Analysis

Case requirements

案例： 无人车上安装有激光雷达，现激光雷达扫描到一点状障碍物并且可以定位障碍物的坐标，请在雷达坐标系下发布障碍物坐标点数据，并在 rviz2 中查看发布结果。

Case Analysis

The above case is a simple topic publishing program. After understanding the coordinate points,geometry_msgs/msg/PointStampedinterface messages, one can directly publish messages through the topic publisher according to a certain logic.

Process Overview

The main steps of program implementation are as follows:

Write the topic publishing implementation;
Edit the configuration file;
Compile;
Execute;
View the running result in rviz2.

For the example, we will implement it using C++ and Python respectively, and both will follow the implementation process described above.

CMakeLists.txt The file needs the following content added:

add_executable(demo03_point_publisher src/demo03_point_publisher.cpp)
ament_target_dependencies(
  demo03_point_publisher
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

Change the content of install in the file to the following:

install(TARGETS demo01_static_tf_broadcaster
  demo02_dynamic_tf_broadcaster
  demo03_point_publisher
  DESTINATION lib/${PROJECT_NAME})

2.5.2.Implementation

/*  
    需求：发布雷达坐标系中某个坐标点相对于雷达（laser）坐标系的位姿。
    步骤：
        1.包含头文件；
        2.初始化 ROS 客户端；
        3.定义节点类；
            3-1.创建坐标点发布方；
            3-2.创建定时器；
            3-3.组织并发布坐标点消息。
        4.调用 spin 函数，并传入对象指针；
        5.释放资源。

*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"

using namespace std::chrono_literals;

// 3.定义节点类；
class MinimalPointPublisher: public rclcpp::Node {
public:
    MinimalPointPublisher(): Node("minimal_point_publisher"),x(0.1){
        // 3-1.创建坐标点发布方；
        point_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>("point",10);
        // 3-2.创建定时器；
        timer_ = this->create_wall_timer(0.1s,std::bind(&MinimalPointPublisher::on_timer, this));
    }
private:
    void on_timer(){
        // 3-3.组织并发布坐标点消息。
        geometry_msgs::msg::PointStamped point;
        point.header.frame_id = "laser";
        point.header.stamp = this->now();
        x += 0.004;
        point.point.x = x;
        point.point.y = 0.0;
        point.point.z = 0.1;        
        point_pub_->publish(point);
    }
    rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr point_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    double_t x;
};

int main(int argc, char const *argv[])
{
    // 2.初始化 ROS 客户端；
    rclcpp::init(argc,argv);
    // 4.调用 spin 函数，并传入对象指针；
    rclcpp::spin(std::make_shared<MinimalPointPublisher>());
    // 5.释放资源。
    rclcpp::shutdown();
    return 0;
}

To create a timer, use the create_wall_time() function, and fill in the time interval and the corresponding callback function.

Execute

In the current workspace, start two terminals. In terminal 1, enter the following command to publish the static coordinate transformation of the radar (laser) relative to the chassis (base_link):

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser

Terminal 2 enters the following command to publish the coordinate point of the obstacle relative to the radar (laser):

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo03_point_publisher

View coordinate frame relationships in rviz2

Reference 5.3.2 Static Broadcaster (Command) Launch the content and configure rviz2, display the coordinate transformation, then add the PointStamped plugin and set its topic to /point. The final display result will be similar to the case demonstration.

Here you can change the ball's transparency and size.

2.6.Summary

Static broadcasts are published only once.

Both dynamic broadcast and coordinate point broadcast are published multiple times.

But in essence, it is topic communication.

3.Coordinate transformation listener

3.1.Case and Analysis

Case 1: In 5.3 Coordinate Transformation Broadcasting , the coordinate system relationship of laser relative to base_link and camera relative to base_link is published. Find the coordinate system relationship of laser relative to camera.

Example 2: In 5.3 Coordinate Transformation Broadcast , the coordinate system relationship of laser relative to base_link was published, and the coordinate point data of the obstacle in the laser coordinate system was published. Please solve for the coordinates of the obstacle in the base_link coordinate system.

Case Analysis

In the above cases, Case 1 involves transformation between different coordinate systems in a multi-coordinate-system scenario, while Case 2 involves transforming the same coordinate point under different coordinate systems. Although the requirements differ, the relevant algorithms have been encapsulated, and we only need to call the relevant API.

Process Overview

The implementation processes of the two cases are similar, and the main steps are as follows:

Write a program to implement coordinate transformation;
Edit the configuration file;
Compile;
Execute.

For the example, we will implement it using C++ and Python respectively, and both will follow the implementation process described above.

Preparation

In the terminal, enter the src directory of the workspace and call the following two commands to create the C++ function package and Python function package respectively.

ros2 pkg create cpp04_tf_listener --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs --node-name demo01_tf_listener

3.2.坐标系变换监听_C++

3.2.1.Case Analysis

Unlike the previous implementation, it needs to be saved to the buffer.

Because the previous broadcast was: send one message, subscribe to one.

However, in coordinate transformation, it is implemented as many-to-one.

Messages published by multiple broadcasts form a coordinate tree, and transformations between different coordinate frames need to be obtained from the coordinate tree.

To form a coordinate tree from multiple broadcaster messages, a buffer must be used to store all messages in the cache buffer.

Why is exception handling needed here?

Because the communication overhead between processes is relatively large and involves latency, the program may be about to start performing transformations, but unfortunately the message has not yet been subscribed to.

If there is no message at all, an exception will be thrown. Only when there is data in the buffer and the coordinates are also converted successfully will the exception no longer be thrown.

3.2.2.Implementation

/*  
  需求：订阅 laser 到 base_link 以及 camera 到 base_link 的坐标系关系，
       并生成 laser 到 camera 的坐标变换。
  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建tf缓存对象指针；融合多个坐标系相对关系为一棵坐标树。
      3-2.创建tf监听器；指定缓存对象，会将所有广播器广播的数据写入缓存。
      3-3.编写定时器，循环实现转换；按照条件查找符合条件的坐标系并生成变换后的坐标帧。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/LinearMath/Quaternion.h"

using namespace std::chrono_literals;

// 3.定义节点类；
class MinimalFrameListener : public rclcpp::Node {
public:
  MinimalFrameListener():Node("minimal_frame_listener"){
    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
    transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    timer_ = this->create_wall_timer(1s, std::bind(&MinimalFrameListener::on_timer,this));
  }

private:
  void on_timer(){
    try
    {
      auto transformStamped = tf_buffer_->lookupTransform("camera","laser",tf2::TimePointZero);
      RCLCPP_INFO(this->get_logger(),"----------------------转换结果----------------------");
      RCLCPP_INFO(this->get_logger(),"frame_id:%s",transformStamped.header.frame_id.c_str());
      RCLCPP_INFO(this->get_logger(),"child_frame_id:%s",transformStamped.child_frame_id.c_str());
      RCLCPP_INFO(this->get_logger(),"坐标:(%.2f,%.2f,%.2f)",
                transformStamped.transform.translation.x,
                transformStamped.transform.translation.y,
                transformStamped.transform.translation.z);

    }
    catch(const tf2::LookupException& e)
    {
      RCLCPP_INFO(this->get_logger(),"坐标变换异常：%s",e.what());
    }

  }
  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
};

int main(int argc, char const *argv[])
{
  // 2.初始化 ROS 客户端；
  rclcpp::init(argc,argv);
  // 4.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<MinimalFrameListener>());
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

Need to fill in the callback function

The core function for implementing coordinate system transformation is lookuptransform().

target_frame is the parent

source_frame is the child

time is the time, generally write the latest moment

Implementation of a function that throws an exception if the buffer is not captured

In this way, the coordinates are successfully transformed, and the transformed coordinates can be printed.

In addition to using try catch to handle conversion exceptions, you can also use the functions under buffer for conversion.

For example, this cantransform can determine whether normal conversion is possible.

This kind of error needs to be changed to a C-style string.

Start three terminals. In terminal 1, enter the following command to publish the static coordinate transformation of the radar (laser) relative to the chassis (base_link):

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id camera --x -0.5 --z -0.4

Terminal 2 enters the following command to publish the static coordinate transformation of the camera relative to the chassis (base_link):

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser--x 0.4 --z 0.2

Enter the following command at terminal 3 to perform coordinate system transformation:

. install/setup.bash 
ros2 run cpp04_tf_listener demo01_tf_listener

Terminal 3 will output the coordinates of the laser relative to the camera; for the specific result, please refer to Case 1.

When only one broadcast is run, it throws an error.

The normal conversion only occurs when all broadcasts are running.

3.3.坐标点变换监听_C++

3.3.1.Implementation Framework

Find the distance from the coordinate point measured by the laser to base_link

The coordinates from laser to point use the point topic, which is different from the topic used by the listener, so it is not easy to subscribe to; a new subscription object needs to be created.

3.3.2.Framework setup

package.xml When creating a feature package, some of the dependent feature packages have already been automatically configured, but in order to implement coordinate point transformation, it is also necessary to add dependency packagestf2_geometry_msgsand

message_filters The modified configuration is as follows:

<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>message_filters</depend>

CMakeLists.txt The modified content of the file is as follows:

find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(message_filters REQUIRED)

add_executable(demo01_tf_listener src/demo01_tf_listener.cpp)
ament_target_dependencies(
  demo01_tf_listener
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
)
add_executable(demo02_message_filter src/demo02_message_filter.cpp)
ament_target_dependencies(
  demo02_message_filter
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "tf2_geometry_msgs"
  "message_filters"
)
install(TARGETS demo01_tf_listener
  demo02_message_filter
  DESTINATION lib/${PROJECT_NAME})

3.3.3.Implementation

/*  
  需求：将雷达感知到的障碍物的坐标点数据（相对于 laser 坐标系），
       转换成相对于底盘坐标系（base_link）的坐标点。

  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建tf缓存对象指针；
      3-2.创建tf监听器；
      3-3.创建坐标点订阅方并订阅指定话题；
      3-4.创建消息过滤器过滤被处理的数据；
      3-5.为消息过滤器注册转换坐标点数据的回调函数。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/// 1.包含头文件；#include <geometry_msgs/msg/point_stamped.hpp>#include <message_filters/subscriber.h>#include <rclcpp/rclcpp.hpp>#include <tf2_ros/buffer.h>#include <tf2_ros/create_timer_ros.h>#include <tf2_ros/message_filter.h>#include <tf2_ros/transform_listener.h>// #ifdef TF2_CPP_HEADERS//   #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>// #else//   #include <tf2_geometry_msgs/tf2_geometry_msgs.h>// #endif#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>using namespace std::chrono_literals;

// 3.定义节点类；class MessageFilterPointListener : public rclcpp::Node
{
public:
  MessageFilterPointListener(): Node("message_filter_point_listener")
  {

    target_frame_ = "base_link";

    typedef std::chrono::duration<int> seconds_type;
    seconds_type buffer_timeout(1);
    // 3-1.创建tf缓存对象指针；
    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
      this->get_node_base_interface(),
      this->get_node_timers_interface());
    tf2_buffer_->setCreateTimerInterface(timer_interface);
    // 3-2.创建tf监听器；
    tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

    // 3-3.创建坐标点订阅方并订阅指定话题；
    point_sub_.subscribe(this, "point");
    // 3-4.创建消息过滤器过滤被处理的数据；
    tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
      point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
      this->get_node_clock_interface(), buffer_timeout);
    // 3-5.为消息过滤器注册转换坐标点数据的回调函数。
    tf2_filter_->registerCallback(&MessageFilterPointListener::msgCallback, this);
  }

private:
  void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr){
    geometry_msgs::msg::PointStamped point_out;
    try {
      tf2_buffer_->transform(*point_ptr, point_out, target_frame_);
      RCLCPP_INFO(
        this->get_logger(), "坐标点相对于base_link的坐标:(%.2f,%.2f,%.2f)",
        point_out.point.x,
        point_out.point.y,
        point_out.point.z);
    } catch (tf2::TransformException & ex) {
      RCLCPP_WARN(
        // Print exception which was caughtthis->get_logger(), "Failure %s\n", ex.what());
    }
  }
  std::string target_frame_;
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
  std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
};

int main(int argc, char * argv[]){
  // 2.初始化 ROS 客户端；
  rclcpp::init(argc, argv);
  // 4.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<MessageFilterPointListener>());
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

Set the above parameter.

Perform data parsing

Use the first overload.

The conversion process will throw an exception, which can be ignored. If the conversion fails, an exception will be automatically thrown on the terminal.

In the current workspace, start three terminals. In Terminal 1, enter the following command to publish the static coordinate transformation of the radar (laser) relative to the chassis (base_link):

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser

Terminal 2 enters the following command to publish the coordinate point of the obstacle relative to the radar (laser):

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo03_point_publisher

At terminal 3, enter the following command to perform coordinate point transformation:

. install/setup.bash 
ros2 run cpp04_tf_listener demo02_message_filter

Terminal 3 will output the coordinates of the coordinate points relative to base_link; for specific results, refer to Case 2.

Publish in sequence

4.Coordinate Transformation Tool

In the TF framework of ROS2, in addition to encapsulating coordinate broadcasting and subscription functions, some tools are also provided to help us improve development and debugging efficiency. This section mainly introduces the use of these tools, which mainly involve two function packages:tf2_rosandtf2_tools.

tf2_rosThe commonly used nodes provided in the package are as follows:

static_transform_publisher: This node is used to broadcast static coordinate transforms; (used many times)
tf2_monitor: This node is used to print the publishing frequency and network latency of all or specific coordinate frames;
tf2_echo: This node is used to print the translation and rotation relationship of a specific coordinate frame.

tf2_toolsThe nodes provided in the package are as follows:

view_frames: This node can generate a PDF file showing coordinate system relationships; the file contains a tree-structured coordinate system graph.

Among the many tools above, the function packagetf2_ros中的static_transform_publishernode in the Static Broadcaster (Command) section has already been described in detail and will not be covered again in this section.

Preparation:

To better demonstrate the use of the tool, please first start several coordinate system broadcast nodes, for example: you can follow static broadcaster (command) and dynamic broadcaster (C++) to broadcast some coordinate system messages.

First terminal:

ros2 run tf2_ros static_transform_publisher --x 0.4 --y 0 --z 0.2 --yaw 0.5 --roll 0 --pitch 0 --frame-id base_link --child-frame-id laser

Second terminal:

ros2 run tf2_ros static_transform_publisher --x -0.5 --y 0 --z 0.4 --yaw 0 --roll 0 --pitch 0 --frame-id base_link --child-frame-id camera

Third terminal:

ros2 run turtlesim turtlesim_node

Fourth terminal:

ros2 run turtlesim turtle_teleop_key

The fifth terminal:

source install/setup.bash
ros2 run cpp03_tf_broadcaster demo02_dynamic_tf_broadcaster

The above is the preparation work.

4.1.1.tf2_monitor

4.1.1.1.1 Print the publishing frequency and network latency of all coordinate frames

Execute the command in the terminal:

ros2 run tf2_ros tf2_monitor

Output result:

This command has a 10-second blocking time because the frequency needs to be measured over an interval.

The static ones show no change, whereas the dynamic publishing frequency does exhibit some variation.

This function will publish again every 10 seconds.

4.1.2.1.2 Print the publishing frequency and network latency of the specified coordinate system

Execute the command in the terminal:

ros2 run tf2_ros tf2_monitor camera laser

Output result:

It is normal to get an error when data cannot be retrieved from the cache at first.

4.2.2.tf2_echo

Print the translation and rotation relationship between the two coordinate systems.

Execute the command in the terminal:

ros2 run tf2_ros tf2_echo world turtle1

Output result:

Translation and rotation amounts will be output.

It can be represented in several ways, such astranslation distance, quaternion, Euler angles in radians, Euler angles in degrees, matrixetc.

When the beta male 🐢🚹 moves, the values also change.

4.3.3.view_frames (common)

Display the coordinate system relationships graphically.

Execute the command in the terminal:

ros2 run tf2_tools view_frames

运行结果：将会在当前目录下生成 frames_xxxx.gv 与 frames_xxxx.pdf 文件，其中 xxxx 为时间戳。打开 pdf 文件显示如下内容：

This node will listen for 5 seconds and generate the corresponding file.

In the image above, there are two coordinate trees, but in an actual project, we can only design one tree and cannot design two coordinate trees.

1.Coordinate transformation

1.1.Introduction and Application Scenarios

1.2.Concept and Role

1.3.Case installation and operation

1.4.Coordinate transformation related messages

1.4.1.geometry_msgs/msg/TransformStamped

1.4.2.geometry_msgs/msg/PointStamped

2.Coordinate transformation broadcasting

2.1.Introduction, Case, and Analysis

2.1.1.1. Case Requirements

2.1.2.2. Case Analysis

2.1.3.3. Process Overview

2.1.4.4. Preparatory Work

2.2.静态广播器_命令行实现

2.2.1.1. Static Broadcaster Tool

2.2.2.2. Static Broadcaster Tool Usage

2.2.3.3. Viewing coordinate frame relationships with rviz2

2.3.静态广播器_C++实现

2.3.1.Framework setup

2.3.2.Broadcast implementation

2.4.动态广播器_C++实现

2.4.1.Framework setup

2.4.2.Broadcast implementation

2.5.坐标点发布_C++实现

2.5.1.Case and Analysis

2.5.2.Implementation

2.6.Summary

3.Coordinate transformation listener

3.1.Case and Analysis

3.2.坐标 系 变换监听_C++

3.2.1.Case Analysis

3.2.2.Implementation

3.3.坐标 点 变换监听_C++

3.3.1.Implementation Framework

3.3.2.Framework setup

3.3.3.Implementation

4.Coordinate Transformation Tool

4.1.1.tf2_monitor

4.1.1.1.1 Print the publishing frequency and network latency of all coordinate frames

4.1.2.1.2 Print the publishing frequency and network latency of the specified coordinate system

4.2.2.tf2_echo

4.3.3.view_frames (common)

5.Practice

5.1.Turtle Follow

5.2.Turtle Escort

6.Summary

3.2.坐标系变换监听_C++

3.3.坐标点变换监听_C++