TF Coordinate Transformation of ROS Advanced Components Part Three
1 data
This article is the third in the TF coordinate transformation series. We use cpp and python to do a homework respectively: the picture below is a smart car, and the whole body has two coordinate systems, one is the base_link coordinate system located in the center of the car body, and the other is The lidar base_laser coordinate system at the top, the relative positions of the two coordinate systems have been marked. Please write a program to convert the point cloud coordinate data described by base_laser detected by lidar into base_link coordinate data in real time.
The reference materials for this article are as follows:
(1) ROS Efficient Advanced Chapter 1 – TF Coordinate Transformation of ROS Advanced Components Part 1
(2) ROS Efficient Advanced Chapter 1 – TF Coordinate Transformation of ROS Advanced Components Part 2
(3) ROS Exploration Summary-22. Set the tf transformation of the robot
(4) "ROS Robot Development Practice" Hu Chunxu Chapter 4 Section 2
2 text
2.1 cpp版learning_tf_homework
(1) Create the learning_tf_homework package and corresponding files
cd ~/catkin_ws/src
catkin_create_pkg learning_tf_homework roscpp rospy tf geometry_msgs
cd learning_tf_homework
mkdir launch scripts
touch launch/start_cpp.launch launch/start_py.launch
touch src/tf_broadcaster.cpp src/tf_listener.cpp
touch scripts/tf_broadcaster.py scripts/tf_listener.p
(2)tf_broadcaster.cpp
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "robot_tf_broadcaster");
ros::NodeHandle nh;
ros::Rate loop_rate(100);
tf::TransformBroadcaster br;
while (nh.ok()) {
// 发布相对于base_link坐标系,base_laser坐标系的 tf 数据
// 相对于base_link坐标系,base_laser坐标系在x 与 z轴分别平移了0.1 和0.2,没有旋转关系
tf::Transform tf_data;
tf_data.setOrigin(tf::Vector3(0.1, 0.0, 0.2));
tf_data.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(tf_data, ros::Time::now(), "base_link", "base_laser"));
loop_rate.sleep();
ros::spinOnce();
}
return 0;
}
(3)tf_listener.cpp
#include <ros/ros.h>
#include <stdlib.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
void tfPointCb(const ros::TimerEvent& ev, const tf::TransformListener& listener) {
// 用随机数模拟收到的激光雷达点云数据,并指定为base_laser坐标描述
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "base_laser";
laser_point.header.stamp = ros::Time();
laser_point.point.x = double(rand())/double(RAND_MAX);
laser_point.point.y = double(rand())/double(RAND_MAX);
laser_point.point.z = double(rand())/double(RAND_MAX);
try {
tf::StampedTransform tf_data;
listener.lookupTransform("base_link", "base_laser", ros::Time(0), tf_data);
ROS_INFO("base_laser to base_link translation transform: (%.2f, %.2f, %.2f)",
tf_data.getOrigin().x(), tf_data.getOrigin().y(), tf_data.getOrigin().z());
// 使用tf::TransformListener::transformPoint接口,实时将base_laser坐标数据转换为base_link坐标数据
geometry_msgs::PointStamped base_point;
// target coordinate, origin point, target point
listener.transformPoint("base_link", laser_point, base_point);
ROS_INFO("base_laser: (%.2f, %.2f, %.2f) --> base_link: (%.2f, %.2f, %.2f) at time %.2f",
laser_point.point.x, laser_point.point.y, laser_point.point.z,
base_point.point.x, base_point.point.y, base_point.point.z,
base_point.header.stamp.toSec());
} catch (tf::TransformException& ex) {
ROS_ERROR("Received an exception trying to transform a point form base_laser to base_link: %s", ex.what());
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "robot_tf_listener");
ros::NodeHandle nh;
// 创建一个带时间缓存的 tf 监听器
// The argument inside the parentheses ros::Duration(10) specifies that the cache size of the listener will be 10 seconds.
// This means that the listener will store transformations from the last 10 seconds.
tf::TransformListener listener(ros::Duration(10));
srand(time(0));
// 使用ros::Timer,用来调度回调函数,监听 tf 数据。第一个参数是设置计时器触发间隔,这里是1秒
// 注意ros::Timer 内部并没有创建线程,而是占用主线程轮询计时器。
ros::Timer timer = nh.createTimer(ros::Duration(1.0), std::bind(tfPointCb, std::placeholders::_1, std::ref(listener)));
ros::spin();
return 0;
}
(4)start_cpp.launch
<launch>
<node
pkg="learning_tf_homework"
type="tf_broadcaster_node"
name="robot_tf_broadcaster"
required="true"
output="screen"
/>
<node
pkg="learning_tf_homework"
type="tf_listener_node"
name="robot_tf_listener"
required="true"
output="screen"
/>
</launch>
2.2 python版learning_tf_homework
(1) tf_broadcaster.py
implements the same logic as tf_broadcaster.cpp
#! /usr/bin/env python3
import rospy
import tf
def main():
rospy.init_node("robot_tf_broadcaster")
tf_br = tf.TransformBroadcaster()
rate = rospy.Rate(100)
while not rospy.is_shutdown():
translate_date = (0.1, 0.0, 0.2)
rotation_data = (0, 0, 0, 1)
tf_br.sendTransform(translate_date, rotation_data, rospy.Time.now(), "base_laser", "base_link")
rate.sleep()
if __name__ == "__main__":
main()
(2) tf_listener.py
implements the same logic as tf_listener.cpp
#! /usr/bin/env python3
import rospy
import tf
import random
from functools import partial
from geometry_msgs.msg import PointStamped
def tfPointCb(event, listener):
laser_point = PointStamped()
laser_point.header.frame_id = "base_laser"
laser_point.header.stamp = rospy.Time(0)
laser_point.point.x = random.random()
laser_point.point.y = random.random()
laser_point.point.z = random.random()
try:
(translation, rotation) = listener.lookupTransform("base_link", "base_laser", rospy.Time(0))
rospy.loginfo("base_laser to base_link translation transform: (%.2f, %.2f, %.2f)" \
%(translation[0], translation[1], translation[2]))
base_point = listener.transformPoint("base_link", laser_point)
rospy.loginfo("base_laser: (%.2f, %.2f, %.2f) --> base_link: (%.2f, %.2f, %.2f) at time %.2f" \
%(laser_point.point.x, laser_point.point.y, laser_point.point.z,
base_point.point.x, base_point.point.y, base_point.point.z,
base_point.header.stamp.to_sec()))
except Exception as e:
rospy.logerr("---> %s" %e)
def main():
rospy.init_node("robot_tf_listener")
tf_listener = tf.TransformListener(rospy.Duration(10))
bind_tf_point_cb = partial(tfPointCb, listener=tf_listener)
timer = rospy.Timer(rospy.Duration(1), bind_tf_point_cb)
rospy.spin()
if __name__ == "__main__":
main()
(3)start_py.launch
<launch>
<node
pkg="learning_tf_homework"
type="tf_broadcaster.py"
name="robot_tf_broadcaster"
required="true"
output="screen"
/>
<node
pkg="learning_tf_homework"
type="tf_listener.py"
name="robot_tf_listener"
required="true"
output="screen"
/>
</launch>
2.3 Compile and run
(1)CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(learning_tf_homework)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
tf
)
catkin_package()
include_directories(
${
catkin_INCLUDE_DIRS}
)
add_executable(tf_broadcaster_node src/tf_broadcaster.cpp)
add_executable(tf_listener_node src/tf_listener.cpp)
target_link_libraries(tf_broadcaster_node
${
catkin_LIBRARIES}
)
target_link_libraries(tf_listener_node
${
catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
scripts/tf_broadcaster.py scripts/tf_listener.py
DESTINATION ${
CATKIN_PACKAGE_BIN_DESTINATION}
)
(2) Compile and run
cd ~/catkin_ws/
catkin_make --source src/learning_tf_homework
source devel/setup.bash
roslaunch learning_tf_homeworkstart_cpp.launch
roslaunch learning_tf_homeworkstart_py.launch
3 Summary
The code in this section is hosted on my github: learning_tf_homework