ROS高效进阶第一章 -- ROS高级组件之 TF坐标变换 其三

1 资料

本文是TF坐标变换系列的第三篇，我们分别使用cpp和python做一个家庭作业：下图是一个智能小车，整个车身有两个坐标系，一是位于车体中心的base_link坐标系，二是位于顶部的激光雷达base_laser坐标系，两个坐标系的相对位置已经标出来了。请编写程序，实时地将激光雷达探测的用 base_laser 描述的点云坐标数据转换为 base_link 坐标数据。

本文参考资料如下：
（1）ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一
（2）ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其二
（3）ROS探索总结-22.设置机器人的tf变换
（4）《ROS机器人开发实践》胡春旭 第4章第二节

2 正文

2.1 cpp版learning_tf_homework

（1）创建 learning_tf_homework 软件包，以及相应的文件

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
实现逻辑同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
实现逻辑同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 编译和运行

（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）编译并运行

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 总结

本节代码托管在本人的github上：learning_tf_homework