ROS Efficient Advanced Chapter 1--TF Coordinate Transformation of ROS Advanced Components

1 data

Starting from this article, we use three blogs to learn TF coordinate changes, TF is transform, and the English of the movie Transformers is transformer.
The reference materials of this article are as follows:
(1) "ROS Robot Development Practice" Hu Chunxu, Chapter 4, Section 2
(2) Right-hand coordinate system: xyz coordinate system and right-hand rule in the robot
(3) Euler angle and quaternion:
Explain in detail the mutual conversion between Euler angles and quaternions
Quaternions and Euler angles (RPY angles)
Conversion between quaternions and Euler angles (Yaw, Pitch, Roll)

2 text

2.1 Right-handed coordinate system

In the field of robotics and autonomous driving, all coordinate systems are right-hand coordinate systems: stretch out your right hand, hold it from the positive direction of the x-axis to the positive direction of the y-axis, and the direction of the thumb is the positive direction of the z-axis.

2.2 Euler angles

In three-dimensional space, Roll, Pitch and Yaw are the three basic angles describing the rotation of objects, also known as Euler angles .
Euler angles are used to describe the rotation of an object relative to its original coordinate system. Taking an airplane as an example, its original three-dimensional coordinate system is: the fuselage is along the x-axis, and the nose is facing the positive direction of the x-axis. According to the right-hand rule, The y-axis is on the left side of the plane and the z-axis is on the top of the plane.
(1) Roll (rolling): The angle at which the object rotates around its own x-axis.

(2) Pitch (pitch): The angle at which the object rotates around its own y-axis.

(3) Yaw (yaw): The angle at which the object rotates around its own z-axis.

Here is a group diagram that can also help understand Euler angles:

2.3 Euler angles and quaternions

Both Euler angles and quaternions are used to describe the rotation of physics in three-dimensional space. Here is a comparison:
(1) Euler angles use three rotation angles, which are intuitive and clear, but there will be a problem of gimbal locking. For details, see: Detailed Explanation of Euler Angle and Quaternion
(2) Quaternion describes the rotation of an object, using the combination of rotation vector + rotation angle, the vector is represented by x, y, z, and the angle is represented by w, that is, q=(x ,y,z,w), that is, quaternions. Moreover, x, y, and z are complex numbers, namely: q=xi+yj+zk+w, i2 = j2 = k2 = -1.
(3) The quaternion has the advantages of high operation efficiency, no gimbal deadlock problem, and convenient interpolation operation. However, since quaternions cannot be intuitively understood in a physical sense, they are very abstract. Therefore, under normal circumstances, people use Euler angles to describe rotations, and when operations are required, they are converted to quaternions .
(4) There is a set of fixed matrix operation conversion formulas between quaternions and Euler angles (I will not delve into it here, and it is really difficult to understand): Conversion of quaternions and Euler angles (Yaw, Pitch, Roll)

2.4 Origin of TF software package

A robot usually has many coordinate systems: hands, feet, head, joints, etc.

An autonomous vehicle (also a robot system) often has many coordinate systems, such as:
(1) Universal Transverse Mercator projection (UTM, GPS coordinates are generally converted to this coordinate value)
(2) lidar coordinate system: lidar point cloud data is described by this coordinate system
(3) radar coordinate system (multiple): same as above
(4) camera coordinates System (multiple): Same as above
(5) Coordinate system of imu:
(6) Car body coordinate system: the origin is located at the center of the rear axle
(7) Local horizontal coordinate system: ENU, commonly known as the world coordinate system
All sensors and GPS of autonomous vehicles The data are all values ​​relative to their own coordinate system. When using it, it needs to be converted to the same coordinate system before it can be used. Generally, the ENU coordinate system is selected, otherwise it will be scattered and invalid data.

Usually, the conversion of two three-dimensional coordinate systems includes translation and rotation. The translation relationship is relatively simple, that is, the movement relationship between the two coordinate origins; the rotation relationship is more complicated, and Euler angles and quaternions are required.
The data conversion of the three-dimensional coordinate system of the robot system is very complicated, and ros provides the TF package to solve this difficult problem, avoiding developers to manually perform coordinate transformation.

2.5 Take turtle_tf as an example to learn TF package

ROS provides turtle_tf as a sample for developers to learn the basic principles of the TF package.
(1) Run the turtle_tf sample

roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key
// 如果rviz无显示，看这个博客：https://www.codestudyblog.com/cs2201ai/40115063750.html
rosrun rviz rviz 'rospack find turtle_tf' /rviz/turtle_rviz.rviz

turtle_tf has designed two turtles. The operator uses the keyboard to control the movement of the first turtle, and the second turtle will follow up actively, as shown in the figure below. (2) The world coordinate system of the three coordinate systems of turtle_tf: the origin is located in the lower left corner of the window, the x-axis starts from the origin and faces to the right; the y-axis starts from the origin and faces to the upper side; according to the right-hand rule, the z-axis is perpendicular to the

screen
, facing the person; since the simulation environment is two-dimensional, the z-axis does not need to be considered.
turtle1 coordinate system: turtle1's own coordinate system, turtle1 is along the x-axis, the head faces the x-axis square, the y-axis is on the left side of turtle1, z-axis is on the top of turtle1 (don't worry about it) turtle2 coordinate system: turtle2's own coordinate system, same
as

turtle1 In rviz, the three coordinate systems look like this (note: the two screenshots are not at the same time):


(3) The logic of the TF package management coordinate system: TF coordinate system tree
The TF package will organize each coordinate system into a coordinate system internally Tree. For example, in the turtle_tf package, the root node of the coordinate system tree is the world coordinate system, and the left and right child nodes are the own coordinate systems of the two turtles. The two turtles each create their own TF broadcaster to broadcast the tf data of their own coordinate system and the world coordinate system, and the TF package can build a coordinate system tree in real time.
Execute the following command to export the TF coordinate system tree of turtle_tf:

// ubuntu20.04执行下面命令会崩溃，解决方式：https://github.com/ros/geometry/pull/193
rosrun tf view_frames

(4) With the TF coordinate system tree, you can obtain the translation and rotation relationship of turtle1 relative to the turtle2 coordinate system in real time by creating a TF listener . Execute the following command:

rosrun tf tf_echo turtle2 turtle1

Run output:

At time 1688002771.829
  // 平移向量，也就是两个坐标原点的平移，即在turtle2坐标系下，turtle1的位置
  - Translation: [-0.403, 0.072, 0.000]   
  // 四元数描述的旋转关系，即在turtle2坐标系下，turtle1的朝向
  - Rotation: in Quaternion [0.000, 0.000, -0.089, 0.996]     
  			  // 弧度描述的欧拉角，因为是二维平面，只有yaw（绕z轴）角有值
              in RPY (radian) [0.000, 0.000, -0.178]        
              // 角度描述的欧拉角  
              in RPY (degree) [0.000, 0.000, -10.174]         

Using the obtained coordinate change relationship, turtle2 can follow turtle1 in real time. For the specific implementation, see Chapter 1 of ROS Efficient Advancement - TF Coordinate Transformation of ROS Advanced Components Part 2 .

2.6 Understanding turtlesim::Pose and geometry_msgs/Twist

In the ROS Efficient Getting Started series , we wrote a lot of samples, all using turtlesim::Pose and geometry_msgs/Twist messages. The topic name of the former is: /turtle1/pose, which is used to describe the position information of turtle1; the topic name of the latter is: /turtle1/cmd_vel, which is used to control the operation of turtle1.
Since we don’t have the knowledge of the coordinate system before, we don’t have a deep understanding of the data fields of these two messages. Here we analyze in depth
(1) turtlesim::Pose is a message that defines the position and orientation of the turtle in the world coordinate system type. It contains the following five fields:

x，y：是world坐标系下的位置
theta：是world坐标系下，乌龟相对于z轴的旋转；
	   如果乌龟头朝向world坐标系x轴正向，则theta为0；
	   逆时针旋转时，theta增加，最大为pi；
	   顺时针旋转时，theta减小，最小为-pi
linear_velocity：线速度，米每秒
angular_velocity：角速度，弧度每秒

(2) The domain of geometry_msgs/Twist is two vectors:

geometry_msgs/Vector3 linear    // turtle相对于自身坐标系的线速度向量
  float64 x     // 沿x轴方向的线速度（m/s），正值为前进，负值为后退
  float64 y     // 沿y轴方向的线速度（m/s），由于turtle永远沿着自身坐标系x轴朝向正方向，不会侧向运动，因此此值一直为0
  float64 z     // 沿z轴方向的线速度（m/s），由于turtle位于二维平面内，不会上下运动，因此此值一直为0
geometry_msgs/Vector3 angular   // turtle相对于自身坐标系的角速度向量
  float64 x     // 绕x轴的角速度（以弧度/秒为单位），由于turtle不会绕x轴旋转，因此为零
  float64 y     // 绕y轴的角速度（以弧度/秒为单位），由于turtle不会绕y轴旋转，因此为零
  float64 z     // 绕z轴的角速度（以弧度/秒为单位），表示turtlez左转（正值），右转（负值）

// 这条命令是让turtle1顺时针画圆，圆半径就是r = v / w = 2.0 m/s / 1.8 rad/s ~= 1.11 m
rostopic pub /turtlesim1/turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, -1.8]'

3 Summary

In this section, we start from mathematics, systematically sort out the knowledge of three-dimensional coordinate changes, and use the turtle_tf that comes with ros to initially learn the principle of the TF software package. Based on the knowledge in this section, we will manually implement a turtle_tf sample in the next section, master the interface calls of the TF library, and master the writing methods of the TF broadcaster and TF listener, see ROS Efficient Advanced Chapter 1 – TF of ROS Advanced Components Coordinate Transformation II