Table of contents
1 What is ROS's TF coordinate transformation TransForm Frame
3 Static coordinate transformation
3.1 C++ clion implements static coordinate transformation
3.1.1 CMakeLists.txt configuration
3.1.2 package.xml configuration
3.1.3 Publishing node establishment
3.1.4 Receive node establishment
3.1.6 Supplement (simple method)
1 What is ROS's TF coordinate transformation TransForm Frame
In the robot system, it is equipped with a variety of sensors, such as lidar and cameras. These sensors can sense the position of objects around the robot, including coordinates, lateral, longitudinal and height distance information. Their role is to assist the robot in accurately locating obstacles. However, not all sensors can directly provide information about the orientation of an object relative to the robotic system or other components. While it is possible to obtain information about an object's orientation relative to a specific sensor, this is not equivalent to information about the object's orientation relative to the entire robotic system or other components. There are limitations in the display of information, as this requires a certain conversion process. This is described in more detail below:
Scenario 1: Radar and Car 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 information, and the obtained coordinates are (x, y). ,z), this coordinate is based on the radar as the reference system. How to convert this coordinate into the coordinates with the car as the reference system?
Scenario 2: An existing robot with a robotic arm (for example: PR2) needs to grip a target object. The current robot head camera can detect the coordinates (x, y, z) of the target object, but the coordinates are based on the camera as the reference system. , and the actual operation of the target object is the gripper of the robotic arm. Currently we need to convert the coordinates into coordinates relative to the gripper of the robotic arm. How to achieve this process?
According to the knowledge we learned in high school, after clarifying the relative relationship between different coordinate systems, we can realize the conversion of any coordinate point between different coordinate systems. However, this calculation implementation is more commonly used, and the algorithm is also a bit complicated. Therefore, the relevant module is directly encapsulated in ROS: coordinate transformation (TF).
If an object is calibrated through the coordinate system in ROS, it will be calibrated through the right-hand coordinate system.
Used in ROS to realize the conversion of 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. The latter is more concise and efficient. The common function packages corresponding to tf2 are:
tf2_geometry_msgs: ROS messages can be converted 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.
2 msg information of coordinate transformationgeometry_msgs/TransformStamped与geometry_msgs/PointStamped
The commonly used msg in coordinate changes are
geometry_msgs/TransformStamped
andgeometry_msgs/PointStamped。我们可以打开终端用rosmsg info查看:
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 the coordinate point information are frequently used.
3 Static coordinate transformation
The so-called static coordinate transformation means that the relative position between the two coordinate systems is fixed.
For example, there is no conversion between the camera and radar in our handheld scanning equipment.
There is a robot model whose core components includemain body and radar, each corresponding to a coordinate system. The origins of the coordinate system are respectively located at the main body and radar. The physical center of the radar, the known displacement relationship between the radar origin and the body origin is as follows: x 0.2 y0.0 z0.5. The radar currently detects an obstacle. The coordinates of the obstacle in the radar coordinate system are (2.0 3.0 5.0). What are the coordinates of the obstacle relative to the subject?
Implementation analysis:
- The relative relationship between coordinate systems can be published through the publisher
- The subscriber subscribes to the relative relationship of the published coordinate system, then passes in the coordinate point information (can be hard-coded), then implements coordinate transformation with the help of tf, and outputs the result
Implementation process:
- Create a new function package and add dependencies
- Writing the publisher implementation
- Write subscriber implementation
- Execute and view the results
Here we use clion to implement it.
3.1 C++ clion implements static coordinate transformation
3.1.1 CMakeLists.txt configuration
Creating project function packages depends on tf2, tf2_ros, tf2_geometry_msgs, roscpp rospy std_msgs geometry_msgs.
cmake_minimum_required(VERSION 2.8.3) project(test) ###################### ### Cmake flags ###################### set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") find_package(catkin REQUIRED COMPONENTS roscpp rospy roslib # msg std_msgs sensor_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs ) catkin_package() include_directories(${catkin_INCLUDE_DIRS}) add_executable(testing main.cpp) target_link_libraries(testing ${catkin_LIBRARIES})
3.1.2 package.xml configuration
These are the simplest ones. If you don’t know how to configure them, it is recommended to rebuild them:
<?xml version="1.0"?> <package> <name>test</name> <version>0.0.0</version> <description>A test</description> <maintainer email="[email protected]">haha</maintainer> <author>HITLHW</author> <license>BSD-3</license> <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <run_depend>roscpp</run_depend> <build_depend>rospy</build_depend> <run_depend>rospy</run_depend> <build_depend>pcl_conversions</build_depend> <run_depend>pcl_conversions</run_depend> <build_depend>std_msgs</build_depend> <run_depend>std_msgs</run_depend> <build_depend>sensor_msgs</build_depend> <run_depend>sensor_msgs</run_depend> <build_depend>geometry_msgs</build_depend> <run_depend>geometry_msgs</run_depend> <build_depend>tf2</build_depend> <run_depend>tf2</run_depend> <build_depend>tf2_ros</build_depend> <run_depend>tf2_ros</run_depend> <build_depend>tf2_geometry_msgs</build_depend> <run_depend>tf2_geometry_msgs</run_depend> </package>
3.1.3 Publishing node establishment
Establish a publishing node:
OK, CMake has configured it for us.
! Haha, the CMake configuration is wrong!
cmake_minimum_required(VERSION 2.8.3) project(test) ###################### ### Cmake flags ###################### set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") find_package(catkin REQUIRED COMPONENTS roscpp rospy roslib # msg std_msgs sensor_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs ) catkin_package() include_directories(${catkin_INCLUDE_DIRS}) add_executable(testing main.cpp) add_executable(static_pub static_pub.cpp) target_link_libraries(testing ${catkin_LIBRARIES}) target_link_libraries(static_pub ${catkin_LIBRARIES})
Our process is as follows:
Static coordinate transformation publisher: Publish location information about the laser coordinate system Implementation process: 1. Include header files 2. Initialize ROS nodes 3. Create a static coordinate transformation broadcaster 4. Create coordinate system information 5. The broadcaster publishes coordinate system information 6.spin()// // Created by liuhongwei on 23-12-1. // /* 静态坐标变换发布方: 发布关于 laser 坐标系的位置信息 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建静态坐标转换广播器 4.创建坐标系信息 5.广播器发布坐标系信息 6.spin() */ // 1.包含头文件 #include "ros/ros.h" #include "tf2_ros/static_transform_broadcaster.h" #include "geometry_msgs/TransformStamped.h" #include "tf2/LinearMath/Quaternion.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"static_brocast"); // 3.创建静态坐标转换广播器 tf2_ros::StaticTransformBroadcaster broadcaster; // 4.创建坐标系信息 geometry_msgs::TransformStamped ts; //----设置头信息 ts.header.seq = 100; ts.header.stamp = ros::Time::now(); ts.header.frame_id = "base_link"; //----设置子级坐标系 ts.child_frame_id = "laser"; //----设置子级相对于父级的偏移量 ts.transform.translation.x = 0.2; ts.transform.translation.y = 0.0; ts.transform.translation.z = 0.5; //----设置四元数:将 欧拉角数据转换成四元数 tf2::Quaternion qtn; qtn.setRPY(0,0,0); ts.transform.rotation.x = qtn.getX(); ts.transform.rotation.y = qtn.getY(); ts.transform.rotation.z = qtn.getZ(); ts.transform.rotation.w = qtn.getW(); // 5.广播器发布坐标系信息 broadcaster.sendTransform(ts); ros::spin(); return 0; }
The annotations are very clear! The main body is base_link and the radar is laser.
We execute this node:
Already have it~
Let’s take a look at the information on this topic:
We can also view through rviz:
3.1.4 Receive node establishment
Create nodes first:
// // Created by liuhongwei on 23-12-1. // /* 订阅坐标系信息,生成一个相对于 子级坐标系的坐标点数据,转换成父级坐标系中的坐标点 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建 TF 订阅节点 4.生成一个坐标点(相对于子级坐标系) 5.转换坐标点(相对于父级坐标系) 6.spin() */ //1.包含头文件 #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" //注意: 调用 transform 必须包含该头文件 int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"tf_sub"); ros::NodeHandle nh; // 3.创建 TF 订阅节点 tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); ros::Rate r(1); while (ros::ok()) { // 4.生成一个坐标点(相对于子级坐标系) geometry_msgs::PointStamped point_laser; point_laser.header.frame_id = "laser"; point_laser.header.stamp = ros::Time::now(); point_laser.point.x = 1; point_laser.point.y = 2; point_laser.point.z = 7.3; // 5.转换坐标点(相对于父级坐标系) //新建一个坐标点,用于接收转换结果 //--------------使用 try 语句或休眠,否则可能由于缓存接收延迟而导致坐标转换失败------------------------ try { geometry_msgs::PointStamped point_base; point_base = buffer.transform(point_laser,"base_link"); ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str()); } catch(const std::exception& e) { // std::cerr << e.what() << '\n'; ROS_INFO("程序异常....."); } r.sleep(); ros::spinOnce(); } return 0; }
That means we are here
First: Create a TF subscription node and cache the subscription information into the buffer
tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer);
Second: To generate a coordinate point, you need to specify which coordinate system it is in.
geometry_msgs::PointStamped point_laser; point_laser.header.frame_id = "laser"; point_laser.header.stamp = ros::Time::now(); point_laser.point.x = 1; point_laser.point.y = 2; point_laser.point.z = 7.3;
The coordinates in the radar coordinate system (laser) are (1, 2, 7.3)
Third: Get the coordinates of this point under base_link and print:
geometry_msgs::PointStamped point_base; point_base = buffer.transform(point_laser,"base_link"); ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());
OK, we have achieved it.
The following are issues that need attention.
3.1.5 Operation results
Run the subscription publishing node:
Success, have you learned it? ?
The final CMakeLists.txt is as follows:
cmake_minimum_required(VERSION 2.8.3) project(test) ###################### ### Cmake flags ###################### set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") find_package(catkin REQUIRED COMPONENTS roscpp rospy roslib # msg std_msgs sensor_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs ) catkin_package() include_directories(${catkin_INCLUDE_DIRS}) add_executable(testing main.cpp) add_executable(static_pub static_pub.cpp) add_executable(static_sub static_sub.cpp) target_link_libraries(testing ${catkin_LIBRARIES}) target_link_libraries(static_pub ${catkin_LIBRARIES}) target_link_libraries(static_sub ${catkin_LIBRARIES})
3.1.6 Supplement (simple method)
When the relative position between coordinate systems is fixed, then the required parameters are also fixed: parent coordinate name, child coordinate system name, x offset, y offset, z offset, x roll angle, y pitch Angle, z-yaw angle, the implementation logic is the same, but the parameters are different, then the ROS system has encapsulated special nodes, and the usage is as follows:
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
Example:
rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
It is also recommended to use this method to directly implement relative information release of static coordinate systems.
We establish the parameters of the camera relative to the chassis:
Let’s check it out: