reference resources
1. Time stamp of TF coordinate broadcast
1.1. Function of timestamp
First clarify what is the coordinate transformation of TF: in fact, it is the pose represented by the child coordinate system in the parent coordinate system, which can also be regarded as the coordinate transformation relationship from the child coordinate system to the parent coordinate system.
By using TF broadcasting, you only need to publish the coordinate relationship between two adjacent coordinate systems at a time. Then TF Subscribers get these adjacent coordinate transformation relationships, and TF can calculate the coordinate transformation relationship between any two coordinate systems.
However, there is a problem: the coordinate system established by TF is generally dynamic coordinate system, that is, the transformation relationship between coordinate systems changes with time. And this coordinate relationship is man-made or sensor detection data, called TF broadcast in the program.
This leads to the problem of time alignment: for example, A manipulator has A big arm and A small arm. The big arm rotates around the shoulder joint and the small arm rotates around the elbow joint. There are two Imus on the boom and jib respectively, which can detect the posture. For example, A detects the posture of the boom relative to the shoulder, and B detects the posture of the jib relative to the boom. Through these two data calculations, we can finally get the posture of the forearm relative to the shoulder. However, the data of the two Imus are always sent. If you want to know the posture of the forearm to the shoulder at 0.5S time, you need to multiply the data at 0.5S time A and 0.5S time B, instead of multiplying the data at other times, because time alignment is required. The same is true when coordinate points are represented in different coordinate systems. Therefore, TF has the concept of timestamp.
1.2. Timestamp in TF coordinate broadcast
Take the coordinate transformation of tf2 (originally TF, later officially improved to tf2, and it is recommended to use tf2) in ROS as an example. The following is the program for TF broadcasting. The program is to subscribe to the turtle posture of turtle SIM, and then publish the TF transformation of turtle coordinate system relative to the world coordinate system according to the turtle posture.
// 1. Include header file
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 5-1. Create TF broadcaster
static tf2_ros::TransformBroadcaster broadcaster;
// 5-2. Create broadcast data (set through pose)
geometry_msgs::TransformStamped tfs;
// |----Header settings
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
// |----Coordinate system ID
tfs.child_frame_id = "turtle1";
// |----Coordinate system relative information setting
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0; // Two dimensional implementation, there is no z in pose, and z is 0
// |---------Quaternion setting
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 5-3. Broadcast data
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2. Initialize ROS node
ros::init(argc,argv,"dynamic_tf_pub");
// 3. Create ROS handle
ros::NodeHandle nh;
// 4. Create subscription object
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
// 5. The callback function processes the subscribed data (implements TF broadcasting)
//
// 6.spin
ros::spin();
return 0;
}
The following three sentences are more important in the program, that is, specify the parent coordinate system, child coordinate system and timestamp of the published tf broadcast. The timestamp here uses ros::Time::now();, That is the time when calling this command, for example, is the result of 150.3S, which means that the tf transformation is coordinate transformation at 150.3S time.
// |----Header settings
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
// |----Coordinate system ID
tfs.child_frame_id = "turtle1";
2. Timestamp of coordinate point
2.1. Time stamp of coordinate point during coordinate transformation
Still taking tf2 as an example, the procedure for monitoring tf broadcast and performing coordinate transformation is as follows. Notice that here is a little different from tf, where the coordinate transformation is to use the buffer cache to listen to the tf broadcast, then call the buffer.transform function, pass the coordinate point and the parent coordinate to transform, then complete the coordinate transformation, that is, point_. base = buffer.transform(point_laser,"world");.
//1. Include header file
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" / / Note: the header file must be included when calling transform
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2. Initialize ROS node
ros::init(argc,argv,"dynamic_tf_sub");
ros::NodeHandle nh;
// 3. Create TF subscription node
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4. Generate a coordinate point (relative to the child coordinate system)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time();
point_laser.point.x = 1;
point_laser.point.y = 1;
point_laser.point.z = 0;
// 5. Convert coordinate points (relative to parent coordinate system)
//Create a new coordinate point to receive the conversion results
//--------------Use try statement or hibernate, otherwise coordinate conversion may fail due to cache reception delay------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"world");
ROS_INFO("Coordinate point relative to world The coordinates of are:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("Program exception:%s",e.what());
}
r.sleep();
ros::spinOnce();
}
return 0;
}
1.2.1. Meaning of time stamp of coordinate point
The following two sentences are more important in the program, that is, they specify that the current coordinate point is relative to the "turnle1" coordinate system.
point_laser.header.frame_id = "turtle1"; point_laser.header.stamp = ros::Time();
So point_ What does laser.header.stamp mean? Is it the timestamp of the current coordinate point?
no Remember that there is no time stamp at the coordinate point, which refers to the time stamp of the coordinate system of turnle1! Or time stamp in tf broadcast. As mentioned earlier, tf broadcasts the coordinate transformation relationship between two coordinate systems, that is, the pose representation of the child coordinate system in the parent coordinate system.
Therefore, specifying a timestamp for the coordinate point here is equivalent to explaining the coordinates in the turnle1 coordinate system at which time the coordinate point is. Because the coordinate representation of the same point may be different in the turnle1 coordinate system at different times (because the coordinate system is moving).
1.2.2. Selection of time stamp of coordinate point
1.2.2.1. Select time stamp in tf broadcast
This choice must be the most accurate. For example, the tf broadcast releases the coordinate transformation at the time of 150.3S, so I also set the coordinate point here to the time of 150.3S, so as to achieve the synchronization of time and space.
But the problem is that we are subscribers to tf broadcasting, and the real-time coordinate transformation is released in tf broadcasting, that is, you don't know how much the 150.3S actually runs, and it is changing all the time, so it's impossible to write it in the program.
1.2.2.2. Select the current time ros::Time()::now()
Obviously, there is the problem of time alignment mentioned earlier (but I guess this problem is not serious, which will be explained later), and a very important problem is that coordinate transformation is impossible at all.
Because when tf broadcasts, it uses the time when it broadcasts, such as 150.3S. Then, after broadcasting, it must take a period of time to be received by subscribers, that is, there is a time difference between broadcasting and subscription, assuming that the time difference is 0.5S. That is, when the time stamp specified by the subscriber is time::now(), you must wait at least 0.5S to get the tf broadcast at this time, or unless the time stamp published by the subscriber is at least 0.5S ahead of the real time. Therefore, if you request coordinate transformation at time::now, an error will be reported that the coordinate transformation cannot be found, as shown below, which indicates that the requested time is in the past 0.018ms, but the latest data in the broadcast is in the past 7.681ms, that is, there is no data in the past 0.018ms, that is, the required tf transformation is later and not yet.
[ERROR] [1287871653.885277559]: You requested a transform that is 0.018 miliseconds in the past, but the most recent transform in the tf buffer is 7.681 miliseconds old. When trying to transform between /turtle1 and /turtle2.
So why does the above say that the problem of time alignment is not very serious?
Personal guess:
- It is not possible to fully align the time
Obviously, as like as two peas in the tf broadcast, they may not be able to achieve strict time alignment. - Look at an official solution (this program is tf1)
Request the coordinate transformation of the current time
listener.lookupTransform("/turtle2", "/turtle1", ros::Time::now(), transform);
An error is reported. The tf broadcast at the current time cannot be found:
[ERROR] [1287871653.885277559]: You requested a transform that is 0.018 miliseconds in the past, but the most recent transform in the tf buffer is 7.681 miliseconds old. When trying to transform between /turtle1 and /turtle2.
Solution: wait for a period of time and know that the broadcast of "current time" I want arrives, that is, the time of this timestamp becomes the time of past.
Note: the current time is quoted here. It can also be seen from the program that ros::Time::now() is obtained and stored in the now variable, then the time waiting for the now arrives, and then the tf transformation of the now is requested.
In other words, I wait for the latest coordinate transformation after now, and then use the coordinate transformation at this time.
For example, now is 180.5S, tf sends a coordinate transformation message every 0.2S, and the delay time is 0.4S.
- The message released by the publisher 180.2S can reach the receiver only at 180.6S, but the timestamp in the message is still 180.2S. Obviously, compared with 180.5S, 180.2S is the coordinate transformation of the past time, which does not meet the requirements; Keep waiting
- The message released by the publisher 180.4S: the receiver can only arrive at 180.8S, but the timestamp in the message is still 180.4S. Obviously, compared with 180.5S, 180.4S is the coordinate transformation of the past time, which does not meet the requirements; Keep waiting
- The message released by the publisher 180.6S: the receiver can only arrive at 181.0S, but the timestamp in the message is still 180.6S. Obviously, compared with 180.5S, 180.6S is the tf broadcast at the latest time that meets the requirements, so if the requirements are met, then use this tf relationship for coordinate transformation.
ros::Time now = ros::Time::now();
listener.waitForTransform("/turtle2", "/turtle1",now, ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1",now, transform);
Therefore, the timestamp is not required to be fully aligned, but a coordinate transformation after and closest to the timestamp will be found in the tf broadcast according to the required timestamp.
- It doesn't seem right after the test
Using the program detection, tf publishes the message at the frequency of 1HZ, and the receiver performs coordinate transformation at the frequency of 3HZ, and the timestamp of the point used each time is the first 2S of the current time. According to the above inference, whether the coordinate transformation in front of or behind the timestamp is used in the 3HZ, the result should be the tf message in the 1HZ message published by tf. However, during the actual coordinate transformation, it is found that for this coordinate transformation request that is not the timestamp published by tf broadcast, tf will calculate, similar to interpolation, to find a coordinate transformation that approximates the requested timestamp.
The test procedure is as follows:
/***************** tf Publish file**********/
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 5-1. Create TF broadcaster
static tf2_ros::TransformBroadcaster broadcaster;
// 5-2. Create broadcast data (set through pose)
geometry_msgs::TransformStamped tfs;
// |----Header settings
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
// |----Coordinate system ID
tfs.child_frame_id = "turtle";
// |----Coordinate system relative information setting
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0; // Two dimensional implementation, there is no z in pose, and z is 0
// |---------Quaternion setting
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 5-3. Broadcast data
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2. Initialize ROS node
ros::init(argc,argv,"tf_pub");
// 3. Create ROS handle
ros::NodeHandle nh;
tf2_ros::TransformBroadcaster broadcaster;
// 5-2. Create broadcast data (set through pose)
geometry_msgs::TransformStamped tfs;
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now(); // If the time is written outside the cycle, the time is the same every time
// |----Coordinate system ID
tfs.child_frame_id = "turtle1";
tfs.transform.rotation.w = 1.0;
ros::Rate rate(1);
double x = 0.0;
while(ros::ok())
{
// |----Coordinate system relative information setting
tfs.transform.translation.x = x;
ROS_INFO("Time Send: %d， x = %.2f", tfs.header.stamp.sec, x);
tfs.header.stamp = ros::Time::now(); // Such a time is the right time
// 5-3. Broadcast data
broadcaster.sendTransform(tfs);
x += 1.0;
rate.sleep();
ros::spinOnce();
}
return 0;
}
/*********** tf Publish function subscription file**********/
//1. Include header file
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" / / Note: the header file must be included when calling transform
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2. Initialize ROS node
ros::init(argc,argv,"dynamic_tf_sub");
ros::NodeHandle nh;
// 3. Create TF subscription node
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(3);
while (ros::ok())
{
// 4. Generate a coordinate point (relative to the child coordinate system)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time::now() - ros::Duration(2.0); // Time before 3s
// point_laser.header.stamp = ros::Time(); // This is the most recent transformation used and has no detection significance
point_laser.point.x = 0;
point_laser.point.y = 0;
point_laser.point.z = 0;
// 5. Convert coordinate points (relative to parent coordinate system)
//Create a new coordinate point to receive the conversion results
//--------------Use try statement or hibernate, otherwise coordinate conversion may fail due to cache reception delay------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"world");
ROS_INFO("Coordinate point relative to world The coordinates of are:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("Program exception:%s",e.what());
ros::Duration(1.0).sleep();
continue;
}
r.sleep();
ros::spinOnce();
}
return 0;
}
/************* Publisher print results************/
[ INFO] [1633167094.481382269]: Time Send: 1633167093， x = 13.00
[ INFO] [1633167095.481386880]: Time Send: 1633167094， x = 14.00
[ INFO] [1633167096.481388511]: Time Send: 1633167095， x = 15.00
[ INFO] [1633167097.481382270]: Time Send: 1633167096， x = 16.00
/************** Subscriber print results*************/
[ INFO] [1633167420.094095828]: Coordinate point relative to world The coordinates of are:(13.60,0.00,0.00)
[ INFO] [1633167420.427425581]: Coordinate point relative to world The coordinates of are:(13.93,0.00,0.00)
[ INFO] [1633167420.760707966]: Coordinate point relative to world The coordinates of are:(14.26,0.00,0.00)
[ INFO] [1633167421.094112750]: Coordinate point relative to world The coordinates of are:(14.60,0.00,0.00)
[ INFO] [1633167421.427433171]: Coordinate point relative to world The coordinates of are:(14.93,0.00,0.00)
[ INFO] [1633167421.760747038]: Coordinate point relative to world The coordinates of are:(15.26,0.00,0.00)
[ INFO] [1633167422.094118597]: Coordinate point relative to world The coordinates of are:(15.60,0.00,0.00)
It can be seen from the above experimental results that the publisher publishes an integer tf change at 1HZ. Because the transformation frequency requested by the subscriber is 3HZ, the result of coordinate transformation is decimal and has the feeling of interpolation.
1.2.2.3. Select the latest time ros::Time() or os::Time(0) in TF broadcast
If such a timestamp is used, the coordinate transformation last published in tf broadcast will be used during coordinate transformation. It is considered that the timestamp of the set point is consistent with the timestamp of the transformation on the left.
This is the most common practice, and it is also recommended by the government, which can generally meet the needs.
1.2.3. Timestamp in other messages
The coordinate points described above are coordinate points with time stamps.
In other messages, there is also a timestamp. For example, the Marker to be used to draw three-dimensional objects in rviz, such as cuboids, spheres, etc. when drawing these objects, it is necessary to specify their pose, which must specify the reference coordinate system and time stamp.
Therefore, the processing method here is the same as the above processing method for coordinate points. Generally, ros::Time() can be used, that is, specify that the attitude of the object to be drawn is relative to the latest coordinate transformation in TF broadcast.
Question:
Whether all the above descriptions are correct remains to be verified