ROS学习笔记：TF坐标变换及编程练习

ROS学习笔记：TF坐标变换及编程练习

关于TF的基本概念及实现方法详见上一篇博文：ROS学习笔记：TF坐标变换及编程详解

本文我们在之前的基础上进行TF的编程练习。

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

我们按照TF的使用流程进行编程：

TF广播器编程

实现流程：

1. 初始化ROS节点
2. 定义一个广播器
3. 创建坐标变换Transform，并初始化相关信息
4. 广播器发布变换信息

程序具体实现如下：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv)
{
    
    
	//初始化ROS节点
	ros::init(argc, argv, "tf_broadcaster");
	ros::NodeHandle n;
	
	//声明一个tf广播器
	tf::TransformBroadcaster br;
	
	while(ros::ok())
	{
    
    
		//创建坐标变换transform
		tf::Transform transform;
		//给定平移变换值
		transform.setOrigin(tf::Vector3(0.1, 0.0, 0.2));
		//给定旋转变换值，这里没有旋转变换，所以定义四元数时无需给定角度
		transform.setRotation(tf::Quaternion(0, 0, 0, 1));
		
		//广播器发布坐标变换
		br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "base_laser"));
	}
	
	return 0;
}

TF监听器编程

实现流程：

1. 初始化ROS节点信息
2. 声明一个TF监听器
3. 创建“base_laser”上的点，并获取相应值
4. 监听获取变换信息，并由“base_laser”上的点转换得到“base_link”上的坐标点

实现细节：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>

int main(int argc, char** argv)
{
    
    
	//初始化ROS节点信息
	ros::init(argc, argv, "tf_listener");
	ros::NodeHandle n;
	
	//创建TF监听器
	tf::TransformListener listener;
	
	ros::Rate rate(10.0);
	while(ros::ok())
	{
    
    
		//在base_laser上声明一个点laser_point，用来转换得到base_link上的点坐标
		geometry_msgs::PointStamped laser_point;
		laser_point.header.frame_id = "base_laser";
		//使用最近可用的转换
		laser_point.header.stamp = ros::Time();
		
		//laser_point检测点获取
		laser_point.point.x = 0.3;
		laser_point.point.y = 0.0;
		laser_point.point.z = 0.0;
		
		try
		{
    
    
			//监听获取tf变换
			listener.waitForTransform("base_link", "base_laser", ros::Time(0), ros::Duration(3.0));
			
			geometry_msgs::PointStamped base_point;
			base_point.header.frame_id = "base_link";
			listener.transformPoint("base_link", laser_point, base_point);
			
			//将坐标变换值进行打印
			ROS_INFO("Change laser_point:(%.2f, %.2f, %.2f) to base_point(%.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("%s", ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		rate.sleep();
	}
	return 0;
}

下图是“geometry_msgs/PoseStamped.h”消息内的内容，声明了2个类型：header和point

header内部

point内部

配置Launch文件

然后配置一下CMakeLists.txt和package.xml文件就可以执行了。

以下是结果