ROS little turtle follows
tf module: used in ROS to convert points or vectors between different coordinate systems.
In ROS, coordinate transformation originally corresponded to tf. However, starting from the hydro version, tf was abandoned and migrated to tf2, which is more concise and efficient. The common function packages corresponding to tf2 are:
tf2_geometry_msgs: can convert ROS messages into tf2 messages.
tf2: Encapsulates common messages for coordinate transformation.
tf2_ros: Provides roscpp and rospy bindings for tf2, encapsulating commonly used APIs for coordinate transformation.
Coordinate system: In ROS, objects are calibrated through the coordinate system. To be precise, they will be calibrated through the right-hand coordinate system.
1. Coordinate msg message
The data carrier msg in the subscription and publishing model is an important implementation. First, you need to understand the commonly used msggeometry_msgs/TransformStamped
in coordinate conversion implementation : and geometry_msgs/PointStamped
The former is used to transmit position information related to the coordinate system, and the latter is used to transmit information about coordinate points in a certain coordinate system. In coordinate transformation, the relative relationship of the coordinate system and coordinate point information are frequently needed.
2. Static coordinate transformation
The so-called static coordinate transformation means that the relative position between the two coordinate systems is fixed.
Create a project function package that depends on tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs
Pay attention to the relationship between the child and parent coordinate systems and the method used by the coordinate transformation function transform() .
3. Dynamic coordinate transformation
The so-called dynamic coordinate transformation means that the relative position between the two coordinate systems changes.
Start turtlesim_node . The form in this node has a world coordinate system (the lower left corner is the origin of the coordinate system), and the turtle is another coordinate system. The keyboard controls the movement of the turtle and dynamically publishes the relative positions of the two coordinate systems.
Create a project function package that depends on tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim
Subscribing to turtle1/pose , you can get the x-coordinate, y-coordinate, offset, linear velocity and angular velocity of the turtle in the world coordinate system.
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,callback);
void callback(const turtlesim::Pose::ConstPtr& pose){
geometry_msgs::TransformStamped tfs;
// |----头设置
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
// |----坐标系 ID
tfs.child_frame_id = "turtle1";
// |----坐标系相对信息设置
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0; // 二维实现,pose 中没有z,z 是 0
}
4.Multiple coordinate transformation
The existing coordinate system, the parent coordinate system world, has two sub-level systems son1, son2, and the relationship between son1 relative to world and son2 relative to world is known. Find the coordinates of the origin of son1 in son2, which are also known. The coordinates of a point in son1 are required to find the coordinates of the point in son2.
Create a project function package that depends on tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim
Use static coordinate transformation to publish the coordinate relationships of son1 and son2 relative to the world.
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
<node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>
Analyze the coordinates of the points in son1 relative to son2
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
tfs.transform.translation.x,
tfs.transform.translation.y,
fs.transform.translation.z
);
Then analyze the coordinates and find the coordinates of the point in son2.
geometry_msgs::PointStamped ps;
ps.header.frame_id = "son1";
ps.header.stamp = ros::Time::now();
ps.point.x = 1.0;
ps.point.y = 2.0;
ps.point.z = 3.0;
geometry_msgs::PointStamped psAtSon2;
psAtSon2 = buffer.transform(ps,"son2");
ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
psAtSon2.point.x,
psAtSon2.point.y,
psAtSon2.point.z
);
5. View the coordinate system relationship
输入下面命令
Check whether the feature package is included
rospack find tf2_tools
If not, please use the following command to install it:
sudo apt install ros-noetic-tf2-tools
After starting the coordinate system broadcast program, run the following command:
rosrun tf2_tools view_frames.py
A frames.pdf file will be generated in the current directory
6. Practical operation of TF coordinate transformation
Two turtles are generated, the middle turtle (A) and the lower left turtle (B). B will automatically run to the position of A. When controlled by the keyboard, it only controls the movement of A, but B can follow A.
input the command
rosrun turtlesim turtlesim_node
Only one little turtle appears at this time, so how to create a second little turtle that is not controlled by the keyboard but follows the movement of the first little turtle? We need to use rosservice, and the topic uses spawn.
After creating the second little turtle, what needs to be done is to subscribe to the pose information of the two little turtles and convert it into coordinate information, and then obtain the coordinate information of the first little turtle relative to the second little turtle. Note here that Don't reverse the coordinate information of turtle1 relative to turtle2. Finally, publish the corresponding speed information to the second turtle to follow the first turtle.
7.TF2 and TF
TF2 has replaced TF. TF2 is a superset of TF. It is recommended to learn TF2 instead of TF.
The TF2 function package has enhanced cohesion. The function packages that TF and TF2 rely on are different. TF corresponds to the tf
package, and TF2 corresponds to tf2
the and tf2_ros
package. In TF2, different types of API implementations are subcontracted.
TF2 implementation is more efficient, such as: TF2 static coordinate implementation, Buffer implementation in TF2 coordinate transformation listener, etc.
Coordinate transformation is an extremely important module in the robot system. In ROS, the TF2 component is specially used to implement coordinate transformation. The specific content of TF2 implementation mainly introduces the following parts:
1. Static coordinate transformation broadcaster can be implemented by coding or by calling the built-in function package (the latter is recommended), and is suitable for relatively fixed coordinate system relationships.
2. Dynamic coordinate transformation broadcaster broadcasts the relative relationship between coordinate systems in a coded manner, which is suitable for variable coordinate system relationships.
3. The coordinate transformation listener is used to listen to the coordinate system messages broadcast by the broadcaster, and can realize the transformation between different coordinate systems or the same point between different coordinate systems.
4. The coordinate system relationship in the robot system is relatively complex. You can also use the tf2_tools toolkit to generate the coordinate system relationship diagram in ros.