ROS | TF坐标系变换的编程实现

1. 创建功能包

在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包，命名为tf_lidar_task，并编译完成。

2. 节点编程

2.1 案例说明

广播并监听机器人的坐标变换，已知激光雷达和机器人底盘的坐标关系，求解激光雷达数据在底盘坐标系下的坐标值。

2.2 TF坐标系广播器编程

在功能包下面的src文件夹目录下创建一个空文件lidar_broadcaster.cpp，并打开所创建的文件，输入以下代码。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
	
int main(int argc, char** argv)
{
	//初始化ROS节点
	ros::init(argc, argv, "robot_tf_publisher");
		
	//创建节点句柄
	ros::NodeHandle n;
		
	//设置TF广播频率为100Hz
	ros::Rate r(100);
		
	//创建tf::TransformBroadcaster类的对象
	tf::TransformBroadcaster broadcaster;

	while(n.ok())
	{
		//广播TF坐标系变换关系到ROS系统中			
		broadcaster.sendTransform( tf::StampedTransform( tf::Transform(tf::Quaternion(0, 0, 0, 1),  tf::Vector3(0.1, 0.0, 0.2)), ros::Time::now(), "base_link", "base_laser"));
    	
		r.sleep();
	}
}

说明：

• 头文件ros/ros.h包含了标准ROS类的声明，在每一个ROS程序中都需要包含它。
• 头文件tf/transform_broadcaster.h用来完成TF树的广播，之后会使用到tf::TransformBroadcaster类的实例。
• main函数中一开始都是类似的，初始化ROS节点，创建节点句柄，从而启动ROS节点。
• ros::Rate r(100);的作用是设置TF坐标系广播频率为100Hz。
• tf::TransformBroadcaster broadcaster;的作用是创建一个tf::TransformBroadcaster类的对象，用来广播坐标系base_link -> base_laser的变换关系。
• broadcaster.sendTransform(tf::StampedTransform())的作用是通过tf::TransformBroadcaster类调用sendTransform()函数，向系统中广播参考系之间的坐标变换关系，其中所广播的变换关系的数据类型是tf::StampedTransform，它包含了四个参数。
  • 第一个参数是存储坐标系之间变换关系的变量，此处tf::Transform是tf::StampedTransform的父类，它包含了两个参数。第一个参数表示坐标的旋转变换，通过tf::Quaternion四元数来存储旋转变换的参数，我们用到的两个参考系之间没有发生旋转变换，因此倾斜角、滚动角和偏航角都是0。第二个参数表示坐标的位移变换，这里我们将base_link作为父节点，base_laser作为子节点，从base_link节点到base_laser节点的变换关系为(x: 0.1m, y: 0.0m, z: 0.2m)，将位移量填入到Vector3向量中。
  • 第二个参数是广播TF变换关系的时间戳，这里使用ros::Time::now()表示当前时间。
  • 第三个参数是传递的父节点坐标系的名称，即base_link。
  • 第四个参数是传递的子节点坐标系的名称，即base_laser。
    tf::StampedTransform及tf::Transform类的定义：
    http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1StampedTransform.html
    http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transform.html
• r.sleep();的作用是为了延时，保证以100Hz的频率广播TF变换关系。

相关资料：http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

2.3 TF坐标系监听者编程

在功能包下面的src文件夹目录下创建一个空文件lidar_listener.cpp，并打开所创建的文件，输入以下代码。

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
	
//当接收到TF消息时，自动调用该回调函数
void transformPoint(const tf::TransformListener& listener)
{
	geometry_msgs::PointStamped laser_point;
	laser_point.header.frame_id = "base_laser";

	laser_point.header.stamp = ros::Time();

	laser_point.point.x = 1.0;
	laser_point.point.y = 0.2;
	laser_point.point.z = 0.0;

	try
	{
		geometry_msgs::PointStamped base_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 from \"base_laser\" to \"base_link\": %s", ex.what());
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "robot_tf_listener");
	ros::NodeHandle n;

	tf::TransformListener listener(ros::Duration(10));

	ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

	ros::spin();
}

说明：

• 头文件ros/ros.h包含了标准ROS类的声明，在每一个ROS程序中都需要包含它。
• 头文件geometry_msgs/PointStamped.h包含了坐标系和时间戳等信息，之后会使用geometry_msgs::PointStamped类创建一个虚拟点。
• 头文件tf/transform_listener.h用于创建TF变换的监听者，之后会使用到tf::TransformListener类的对象，该对象会自动订阅ROS中的TF消息，并且管理所有的变换关系数据。
• transformPoint(const tf::TransformListener& listener)该回调函数每当接收到TF消息时，都会被自动调用。在回调函数中，我们需要完成数据从坐标系base_laser到base_link的坐标变换。

geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "base_laser";

laser_point.header.stamp = ros::Time();

laser_point.point.x = 1.0;
laser_point.point.y = 0.2;
laser_point.point.z = 0.0;

• 这里创建了一个geometry_msgs::PointStamped类型的虚拟点，该类型包含标准的header和point两种消息结构，因此可以在消息中添加发布数据的时间戳、参考系的ID以及该点的坐标。其中ros::Time()表示使用缓冲中最新的TF数据，这里不使用ros::Time::now()是因为每个监听器都有一个缓冲区，存储来自不同TF广播的所有坐标变换。当广播器发送TF变换时，数据进入缓冲区需要一段时间（通常是几毫秒）。因此如果在当前时刻就请求TF变换数据，缓冲区中还提取不到当前发布的变换，您应该等待一段时间，直到数据到达。
  PointStamped的消息定义：
  http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html

try
{
	geometry_msgs::PointStamped base_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());
}

• 这里使用tf::TransformListener类的对象中的transformPoint()函数，将虚拟点的坐标数据从base_laser参考系转换到base_link参考系下，该函数包含三个参数。

  • 第一个参数是需要转换到的参考系ID，即base_link。
  • 第二个参数是需要转换的原始数据，即上面创建的虚拟点的信息。
  • 第三个参数是存储TF变换完成后的数据的变量。

  该函数执行完毕后，base_point中就包含了变换完成后的点坐标了，最后通过ROS_INFO()将坐标信息打印在ROS终端界面。

catch(tf::TransformException& ex)
{
	ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
}

• 如果由于某种原因导致base_lase -> base_link的转换不可用（tf_broadcaster未运行等），则在执行transformPoint()时就会出现错误。为了确保能够正常处理，我们将捕获异常并为用户打印错误信息。ex.what()是tf::TransformException类的一个方法，它返回一个指向与异常相关的内容的C字符串的指针。此处必须用try-catch语句，否则会报错。
• main函数中一开始都是类似的，初始化ROS节点，创建节点句柄，从而启动ROS节点。
• tf::TransformListener listener(ros::Duration(10))的作用是通过tf::TransformListener创建一个TF变换的监听者，形参中的ros::Duration()函数表示所存储的TF变换数据的时间长度，TF功能包最大可以存储10s的数据，10s之后就会将之前的消息丢弃掉，这里将数据存储的时间长度设置为10s。
• tf::TransformListener类的定义：
  http://docs.ros.org/diamondback/api/tf/html/c++/classtf_1_1TransformListener.html#a7e41bd5e244f92709e22bffa2860605d
• ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); 此处创建了一个timer对象，通过ros::Duration()函数设置回调函数transformPoint()的执行频率为1Hz，通过boost::bind()函数将回调函数transformPoint()的参数与listener对象绑定，而当某些函数对象或值参数很大，拷贝代价很高，或者无法拷贝时，boost::bind()的使用就会受到限制，因此使用boost::ref()引用传递对象。
• ros::spin();的作用是让程序进入自循环的挂起状态，从而让程序以最好的效率监听TF变换并调用回调函数。

3. 配置与编译

3.1 在CMaKeLists.txt中添加编译选项

将如下位置中CATLIN_DEPENDS前面的“#”去掉，使能相关的依赖项。

在如下位置进行配置，add_executable(lidar_broadcaster src/lidar_broadcaster.cpp)的作用是将src文件夹下的lidar_broadcaster.cpp文件编译成名为lidar_broadcaster的可执行文件。target_link_libraries(lidar_broadcaster ${catkin_LIBRARIES})的作用是将lidar_broadcaster可执行文件与ROS相关的库链接。

add_executable(lidar_broadcaster src/lidar_broadcaster.cpp)
add_executable(lidar_listener src/lidar_listener.cpp)

target_link_libraries(lidar_broadcaster ${catkin_LIBRARIES})
target_link_libraries(lidar_listener ${catkin_LIBRARIES})

3.2 编译文件

在/ROS_ws文件夹路径下打开一个新的终端，输入以下代码进行编译。

$ catkin_make

编译完成后，输入以下代码运行主节点。

$ roscore

打开一个新的终端，配置环境变量后，输入以下代码运行TF广播器。

$ rosrun tf_lidar_task lidar_listener

打开一个新的终端，配置环境变量后，输入以下代码运行TF监听者。

$ rosrun tf_lidar_task lidar_broadcaster

若想停止运行，关闭终端，使用快捷键Ctrl+c即可。

4. 话题可视化

在src文件夹路径下打开一个新的终端，输入以下代码。

$ rosrun tf view_frames

由此可以生成一个名为frames的pdf文件，直观地看到TF变换时的坐标系关系。

$ rosrun rqt_tf_tree rqt_tf_tree

该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。

$ rostopic echo /tf

该命令的作用是以TransformStamped消息类型的数组显示所有父子frame的位姿转换关系。

$ rosrun tf tf_echo base_laser base_link

该命令的作用是查看任意两个frame之间的变换关系，终端将会持续的显示当前源坐标系和目标坐标系的位姿变换关系。