Coordinate Transformation TF

coordinate transformation

Introduction and Application Scenarios

里程计ODOM

惯性计IMU

激光雷达Laser

摄像头Camera

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

The horizontal and vertical distances between the LiDAR and the center or edge of the car, as well as the distance from the LiDAR to the wall and the distance from the car to the wall.

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

The above can be calculated using TF.

Concept and Function

TF uses a right-handed coordinate system.

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

Case Installation and Running

Install the Turtle example:

# 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, you need to install a Python package called transforms3d, which provides quaternion and Euler angle transformation functions 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 will start the turtlesim_node node, which comes with a small turtle named turtle1. In addition, it will spawn a new turtle named turtle2, and turtle2 will move to turtle1's position.

Enter the following command in terminal 2:

ros2 run turtlesim turtle_teleop_key

In this terminal, you can control the movement of turtle1 using the keyboard, and turtle2 will follow turtle1's movement (refer to Case 1 in the introduction section).

A male turtle 🐢🚹 will follow the movement of the turtle ahead 🐢.

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, while the subscriber can listen to these messages and perform transformations between different coordinate frames. Clearly, as introduced earlier, interface messages serve as the data carrier and play a crucial role in the topic communication model. This section will introduce two commonly used interface messages in coordinate transformation: geometry_msgs/msg/TransformStamped and geometry_msgs/msg/PointStamped.

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

geometry_msgs/msg/TransformStamped

View the interface definition using 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

Describing the motion of an object generally involves six degrees of freedom: X, Y, Z, Yaw, Pitch, and Roll.

Three translations, three rotations:

Vector3 translation represents 3 translations

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

(Q: Why use quaternions instead of Euler angles? A: Because calculations with Euler angles can lead to gimbal lock, quaternions are chosen instead to avoid that drawback.)

3 translations in meters

3 rotations in radians

Quaternions, similar to Euler angles, are used to represent the relative orientation of coordinate systems.

For specific conversion details, see Section 18.3 of the DJI Development Board Type C Embedded Software Tutorial Document.pdf.

Specific transformation algorithm (Mahony algorithm) (the TF2 library in ROS2 also has a specific transformation algorithm):

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

From the perspective of the right-hand coordinate system, N2 is translated 1m along the X-axis relative to N1 (one grid represents 1 meter).

Rotation

geometry_msgs/msg/PointStamped

View the interface definition using 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 point in three dimensions

Coordinate transformation broadcast

Introduction and Case Studies and Analysis

There are two main types of coordinate system relationships: static coordinate system relationships and dynamic coordinate system relationships.

The so-called static coordinate system relationship refers to a fixed relative position between two coordinate systems. For example, components such as radar and cameras on a vehicle are typically 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, represents 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, or different vehicles in a multi-vehicle formation, can all move. In such cases, the relative relationship between the coordinate system of a robotic arm's joint or gripper and the vehicle chassis coordinate system, or between different vehicle coordinate systems, is a dynamic relationship.

This section will primarily introduce how to implement static coordinate transform broadcasting and dynamic coordinate transform broadcasting. Additionally, this section will also demonstrate how to publish coordinate point messages.

The LiDAR laser and camera are statically positioned relative to the chassis, while the robot arm's end effector and the robot's position are dynamic.

1. Case Requirements

Case 1: There is an unmanned vehicle with a fixed radar and camera mounted on its chassis. The vehicle chassis, radar, and camera each have their own coordinate systems, with the origin of each system located at its geometric center. It is known that the three-dimensional translation of the radar coordinate system relative to the chassis coordinate system is: 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 translation of the camera coordinate system relative to the chassis coordinate system is: -0.5 meters in the x-direction, 0 meters in the y-direction, and 0.4 meters in the z-direction, with no rotation. Broadcast the relative coordinate relationships between the radar and the chassis, and between the camera and the chassis, and view the broadcast results in rviz2.

Case 2: Start turtlesim_node, where the window has a world coordinate system (origin at the bottom-left corner), and the turtle is another coordinate system. The turtle can be controlled via keyboard. Dynamically publish the relative relationship between the turtle's coordinate system and the world coordinate system.

2. Case Analysis

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

1. How to broadcast coordinate frame relationships;
2. How to display coordinate frame relationships using rviz2.

3. Process Overview

The process for implementing static or dynamic coordinate transformations through coding is similar, with the main steps as follows:

1. Write the broadcast implementation.
2. Edit the configuration file.
3. Compile;
4. execute
5. View the coordinate frame relationships in rviz2.

We will implement the case using C++ and Python separately, both following the implementation process described above.

Additionally: It should be noted that, besides being implemented programmatically, static broadcasters also have built-in tools in tf2 that allow publishing static coordinate frame relationships without coding — simply by running a node and passing parameters that represent the relative relationships between coordinate frames (i.e., via the command line; if a command-line option is available, it should be preferred). Dynamic broadcasters, however, do not offer similar tools (meaning they must be implemented in code).

4. Preparation

In the terminal, navigate to the src directory of the workspace and run the following two commands to create C++ packages respectively.

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

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

The tf2_ros package contains broadcast objects.

geometry_msgs message carriers

The turtlesim package is used to obtain the turtle's 🐢 pose.

Static Broadcaster Command Line Implementation

1. Static Broadcaster Tool

In the tf2_ros package, an executable file named static_transform_publisher is provided. This file can be used to directly broadcast static coordinate frame relationships, and its usage syntax is as follows.

Format 1:

Publish a static coordinate transform to tf2 using x/y/z offsets in meters and roll/pitch/yaw in radians (which refer to rotations around the x/y/z axes, respectively).

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 are mandatory.

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

Offset: X, Y, Z

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

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

Format 2:

Publish a static coordinate transform to tf2 using x/y/z offsets 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 two formats above, aside from --frame-id used to represent the parent coordinate system and --child-frame-id used to represent the child coordinate system, all other parameters are optional. If a specific option is not specified, the default value will be used directly.

2. Using the Static Broadcaster Tool

Open two terminals. In terminal 1, run the following command to publish the static coordinate transform of the laser relative to the base_link (coincident):

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 parent reference frame is selected.

3. Viewing coordinate frame relationships in rviz2

Open a new terminal, and use the command rviz2 to launch rviz2 and configure the relevant plugins to view coordinate transform messages.

1. Set the Fixed Frame in Global Options to base_link.
2. Click the Add button to add the TF plugin.
3. Check the "show names" option in the TF plugin.

The coordinate transformation relationships 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 transform of the laser relative to the 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

In terminal 2, enter the following command to publish the static coordinate transform of the camera relative to the 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

Static Broadcaster_C++ Implementation

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})

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;
}

The parameters must be passed correctly, otherwise an error will be thrown.

Function to create an organization and publish data.

Use the simplest overload.

Now 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 a float floating-point type.

x() and getx() both retrieve the x component of a quaternion. (The same applies to y, z, and w.)

In the terminal, navigate to the current workspace and compile the package:

colcon build --packages-select cpp03_tf_broadcaster

In the current workspace, open two terminals. In Terminal 1, enter the following command to publish the static coordinate transform of the laser relative to the 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

Refer to the Static Broadcaster (Command) content to launch and configure rviz2, with the final execution result similar to Case 1.

The essence is topic-based communication, but what exactly is the topic of this topic-based communication?

By looking at the source code of this class, we can see that the topic is /tf_static.

There are two publishers and one subscriber.

These two are the publishers.

This is the subscriber.

Dynamic Broadcaster_C++ Implementation

Framework setup

The CMakeLists.txt file needs to add the following content:

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"
)

Update the install in the file to the following content:

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

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;
}

Entry parameters:

Parameter 1: Topic Name

Parameter 2 QoS

Parameter 3 callback function

Controlling the turtle's movement will also make it move in Rviz2.

Coordinate Point Publishing_C++ Implementation

Case Studies and Analysis

Case Requirements

Case: An unmanned vehicle is equipped with a LiDAR sensor. The LiDAR scans a point-shaped obstacle and can locate its coordinates. Please publish the obstacle's coordinate point data in the LiDAR coordinate system and view the published result in rviz2.

Case Study

The above example is a simple topic publishing program. After understanding the coordinate point geometry_msgs/msg/PointStamped interface message, the topic publisher can directly publish messages according to a certain logic.

Process Overview

The main steps for program implementation are as follows:

1. Write the topic publishing implementation.
2. Edit the configuration file.
3. Compile;
4. execute
5. View the running results in rviz2.

We will implement the case using C++ and Python separately, both following the implementation process described above.

The CMakeLists.txt file needs to add the following content:

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

Update the install in the file to the following content:

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

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, which requires a time interval and a corresponding callback function.

Execute

In the current workspace, open two terminals. In Terminal 1, enter the following command to publish the static coordinate transform of the laser relative to the 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

In terminal 2, enter the following command to publish the coordinates of the obstacle relative to the radar (laser):

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo03_point_publisher

Viewing coordinate frame relationships in rviz2

Refer to 5.3.2 Static Broadcaster (Command) to launch and configure rviz2. After displaying the coordinate transform, add the PointStamped plugin and set its topic to /point. The final display result should be similar to the example demonstration.

You can change the ball's transparency and size here.

Summary

A static broadcast is published only once.

Dynamic broadcasting and coordinate point broadcasting are both published multiple times.

But essentially, it's topic communication.

Coordinate transformation monitoring

Case Studies and Analysis

Case 1: In 5.3 Coordinate Transformation Broadcasting, the coordinate frame relationships of laser relative to base_link and camera relative to base_link are published. Solve for the coordinate frame relationship of laser relative to camera.

Case 2: In 5.3 Coordinate Transform Broadcast, the coordinate relationship of laser relative to base_link was published, along with the coordinate point data of an obstacle in the laser coordinate system. Solve for the coordinates of that obstacle in the base_link coordinate system.

Case Study

In the above cases, Case 1 involves transformations between different coordinate systems in a multi-coordinate system scenario, while Case 2 requires transforming the same coordinate point under different coordinate systems. Although the requirements differ, the relevant algorithms are already encapsulated, so we only need to call the corresponding APIs.

Process Overview

The implementation process for both cases is similar, with the main steps as follows:

1. Write a coordinate transformation program to implement;
2. Edit the configuration file.
3. Compile;
4. Execute.

We will implement the case using C++ and Python separately, both following the implementation process described above.

Preparation

In the terminal, navigate to the src directory of your workspace and run the following two commands to create a C++ package and a Python package respectively.

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

Coordinate System Transformation Listener_C++

Case Study

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

Because the previous broadcast was publishing one and subscribing to one.

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

Multiple broadcast messages form a coordinate tree, from which transformations between different coordinate frames are obtained.

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

Why is exception handling needed here?

Because the communication overhead between processes is relatively high and introduces latency, the program may need to start performing a transformation, but unfortunately the message hasn't been subscribed to yet.

No messages at all, just throwing exceptions. It won't stop throwing exceptions until there's data in the buffer and the coordinates have been successfully transformed.

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;
}

To fill in the callback function.

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

target_frame is the parent frame.

The source_frame is the child level.

time refers to the current time, typically the most recent moment.

If the buffer fails to capture, the function implementation that throws an exception.

The coordinate conversion was successful, and the converted 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, cantransform can determine whether a normal conversion is possible.

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

Open three terminals. In terminal 1, enter the following command to publish the static coordinate transform of the laser relative to the base_link:

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

In terminal 2, enter the following command to publish the static coordinate transform of the camera relative to the base_link:

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

In terminal 3, enter the following command to perform coordinate system transformation:

. install/setup.bash 
ros2 run cpp04_tf_listener demo01_tf_listener

Terminal 3 will output the coordinates of laser relative to camera. For specific results, please refer to Case 1.

When running only one broadcast, it throws an error.

Only when all broadcasts are running will the conversion proceed normally.

Coordinate Point Transform Listener_C++

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, making it difficult to subscribe. A new subscription object needs to be created.

Framework setup

When creating a functional package, some of the required dependencies are automatically configured in package.xml. However, to implement coordinate point transformations, you still need to add the dependency packages tf2_geometry_msgs and

message_filters, the modified configuration content 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>

The modified content of the CMakeLists.txt 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})

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 may throw an exception, but you can ignore it. If the conversion fails, an exception will be automatically thrown in the terminal.

In the current workspace, open three terminals. In terminal 1, enter the following command to publish the static coordinate transform of the laser relative to the 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

In terminal 2, enter the following command to publish the coordinates of the obstacle relative to the radar (laser):

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo03_point_publisher

In terminal 3, execute the following command to perform coordinate point transformation:

. install/setup.bash 
ros2 run cpp04_tf_listener demo02_message_filter

Terminal 3 will output the coordinate point relative to base_link. For specific results, please refer to Case 2.

Publish in order.

Coordinate Transformation Tool

In the ROS2 TF framework, in addition to encapsulating coordinate broadcasting and subscription functionality, it also provides some tools that can help us improve development and debugging efficiency. This section mainly introduces the use of these tools, which primarily involve two packages: tf2_ros and tf2_tools.

The commonly used nodes provided in the tf2_ros package are as follows:

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

The nodes provided in the tf2_tools package are as follows:

• view_frames: This node can generate a PDF file that displays the relationship between coordinate frames. The file contains a tree-structured coordinate frame graph.

Among the many tools mentioned above, the static_transform_publisher node in the tf2_ros package has already been detailed in the Static Broadcaster (Command) section, so it will not be covered again in this section.

Preparation:

To better demonstrate the use of the tool, please start several coordinate frame broadcasting nodes first. For example, you can broadcast some coordinate frame messages using a static broadcaster (command) and a dynamic broadcaster (C++).

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

The fourth terminal:

ros2 run turtlesim turtle_teleop_key

Fifth terminal:

source install/setup.bash
ros2 run cpp03_tf_broadcaster demo02_dynamic_tf_broadcaster

The above is the preparation work.

1.tf2_monitor

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

Running result:

This command has a 10-second blocking time because it needs to measure frequency within an interval.

Static ones remain unchanged, while the publishing frequency of dynamic ones varies to some extent.

This function will publish again every 10 seconds.

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

Running result:

It's normal to get errors when data can't be fetched from the cache at first.

2.tf2_echo

Print the translation and rotation relationship between two coordinate systems.

Execute the command in the terminal:

ros2 run tf2_ros tf2_echo world turtle1

Running result:

The translation and rotation amounts will be output.

It can be expressed in several ways, such as translation distance, quaternion, Euler angles in radians, Euler angles in degrees, matrix, etc.

When the turtle man 🐢🚹 moves, the value also changes.

3. view_frames (commonly used)

Display the coordinate system relationships graphically.

Execute the command in the terminal:

ros2 run tf2_tools view_frames

Result: The files frames_xxxx.gv and frames_xxxx.pdf will be generated in the current directory, where xxxx is a timestamp. Opening the pdf file displays the following content:

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

The image above shows two coordinate trees, but in an actual project, we can only design one tree, not two.

Practice

Turtle following

Turtle Escort

Summary