ROS——TF坐标变换

 

 TF功能包

创建功能包

cd ~/catkin_ws/src
catkin_creat_pkg learning_tf roscpp rospy tf turtlesim
//如果此时依赖包已有tf，后文中CMakeLists文件中的find_package内则不需要添加tf
//编译功能包
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

TF是一个让用户随时间跟踪多个坐标系的功能包，它使用树形结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间、坐标系间完成点、向量等坐标的变换。

TF可以在分布式系统中进行操作，也就是说，一个机器人系统中所有的坐标变换关系，对于所有的节点组件都是可用的，所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据

使用TF功能包需要完成以下两个步骤

1、监听TF变换

接受并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系

2、广播TF变换

向系统中广播坐标系之间的坐标变换关系。系统中可能存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入TF树中，不需要再进行同步。

TF编程实现

安装功能包

sudo apt-get install ros-noetic-turtle-tf
//我的ros安装版本是noetic，不同的ros版本需要对应不同的功能包安装命令
//查看ros安装版本
roscore
//另起终端
rosparam list
rosparam get /rosdistro

安装功能包完成后执行以下命令

roslaunch turtle_tf turtle_tf_demo.launch
//另起新终端
rosrun turtlesim turtle_teleop_key

然后就可以通过键盘方向键来控制小海龟移动，注意鼠标此时要放在新终端上

 

//创建一个TF监听器监听5s，得到TF结构关系图
rosrun tf view_frames
//查看节点信息
rqt_graph				

                                                     可视化的TF树信息

 使用tf_echo工具在TF树中查找乌龟坐标系之间的变换关系

rosrun tf tf_echo turtle turtle2

也可以通过rviz的图形界面更加形象的看到这三者之间的坐标关系

rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

创建TF广播器

实现功能：创建TF广播器，创建坐标变换值并实时发布坐标变换

——初始化ROS节点，并订阅 turtle 的位置消息；
——循环等待话题消息，接收到之后进入回调函数，该回调函数用以处理并发布坐标变换；
——在该回调函数内部定义一个广播器；
——根据接收到的小海龟的位置消息，创建坐标变换值
——通过定义的广播器发布坐标变换
 

#include 
#include 
#include 

std::string turtle_name;

void PoseCallBack(const turtlesim::PoseConstPtr& msg)
{
	//定义一个tf广播器
	static tf::TransformBroadcaster br;
	
	//根据turtle的位置信息，得到其相对于世界坐标系的变换
	tf::Transform transform;
	transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
	tf::Quaternion q;
	q.setRPY(0,0,msg->theta);
	transform.setRotation(q);
	
	//tf广播器发布2只海龟相对于世界坐标系的坐标变换
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
	//初始化节点
	ros::init(argc, argv, "my_tf_broadcaster");
	
	//判断并且获取turtle_name
	if(argc != 2)
	{
		ROS_ERROR("Need a turtle name!");
		return -1;
	}
	turtle_name = argv[1];
	
	//订阅turtle的pose信息
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe(turtle_name + "/pose", 10, &PoseCallBack);
	
	ros::spin();
	
	return 0;
}

TF监听器

实现功能：创建TF监听器，创建第二只海龟，监听坐标变换并发布运动控制指令使第二只海龟向第一只海龟运动

——初始化ROS节点，并向MASTER注册节点信息；
——通过服务调用产生第二只海龟；
——创建turtle2的速度控制发布器；
——创建tf监听器并监听turtle2相对于turtle1的坐标变换；
——根据坐标变换发布速度控制指令；

#include 
#include 
#include 
#include 

int main(int argc, char** argv)
{
	//初始化节点
	ros::init(argc, argv, "my_tf_listener");
	ros::NodeHandle n;
	
	//service调用产生第二只海龟
	ros::service::waitForService("spawn");
	ros::ServiceClient add_turtle = n.serviceClient("spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);
	
	//定义速度控制指令的消息发布者
	ros::Publisher turtle_vel = n.advertise("turtle2/cmd_vel", 10);
	
	//定义一个tf监听器
	tf::TransformListener listener;
	
	ros::Rate rate(10.0);
	
	while(ros::ok())
	{
		//申明一个空的坐标变换
		tf::StampedTransform transform;
		try
		{
			//监听turtle2和turtle1的坐标变换
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
		}
		catch(tf::TransformException &ex)
		{
			ROS_ERROR("%s", ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		//根据坐标变换计算得出turtle2的角速度和线速度，并发布该消息
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(),2) + pow(transform.getOrigin().y(),2));
		
		turtle_vel.publish(vel_msg);
		
		rate.sleep();
	}

	return 0;
}

完成后打开CMakeLists文件添加编译规则

 

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp) 
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES}) 

同时在find_package中添加TF依赖

 然后进行功能包的编译

cd ~/catkin_ws
catkin_make

编译完成后进行如下操作

roscore //启动ROS Master
rosrun turtlesim turtlesim_node //启动海龟仿真器
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
//发布海龟turtle1的坐标系关系
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
//发布海龟turtle2的坐标系关系
rosrun learning_tf turtle_tf_listener
//启动自定义节点
rosrun turtlesim turtle_teleop_key
//启动海龟控制节点

完成后如下所示