07 ROS的TF坐标管理工具

一、TF介绍

TF 即 Transform 转换之意
通过坐标变换可以得到一个机器人中不同坐标系之间的关系
A坐标系下的位置和姿态可以通过平移和旋转，转化成B坐标系下的位姿
订阅TF消息的节点将会缓冲一份一段时间内所有坐标系变换关系的数据

二、使用TF功能包的方式

1. 监听TF变换 Listen

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

2. 广播TF变换 Broadcast

向系统内广播坐标系之间的变换关系
广播可以将坐标关系存入TF树，不需要再与其他不同部分同步

定义TF广播器
创建坐标变化值并封装
发布坐标变换

三、TF工具的使用

1. 查看TF树中所有坐标的发布状态

<source_frame> <target_frame>为可选参数
可以单独查看两个坐标之间的发布状态

rosrun tf tf_monitor <source_frame> <target_frame>

2. 查看两个坐标系的变换关系

rosrun tf tf_echo turtle1 turtle2

结果：打印出了用RPY和四元数两种方式描述坐标系之间的关系的数据

At time 1647863106.937

• Translation: [0.000, 0.000, 0.000]
• Rotation: in Quaternion [0.000, 0.000, 0.243, 0.970]
  in RPY (radian) [0.000, -0.000, 0.491]
  in RPY (degree) [0.000, -0.000, 28.144]

3. 发布相对静态坐标系的坐标变换

设置两相对静止坐标系间的偏移参数和旋转参数
两种表示方式
1.弧度制 2.使用四元数表示旋转角度

绕z轴的偏航角yaw 绕y轴的俯仰角pitch 绕x轴的翻滚角roll

rosrun tf static_transform_publisher x y z yaw pitch roll father_frame_id child_frame_id period_in_ms

rosrun tf static_transform_publisher x y z qx qy qz qw father_frame_id child_frame_id period_in_ms

在launch文件中使用该命令

<launch>
	<node 
		pkg="tf"
		type="static_transform_publisher"
		name="link1_to_link2_TF_Broadcaster"
		args="1 0 0 0 0 0 1 link1_parent link1 100"
	/>
</launch>

4. 可视化查看TF关系树

可视化TF树并生成pdf文件

rosrun tf view_frames

pdf文件可以直接查看，也可以

evince frames.pdf

5. 三维坐标可视化工具Rviz

可以用三维坐标可视化工具Rviz来查看turtle_tf例程中的两龟关系

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

四、创建TF广播器 TF Broadcaster

广播器将TF信息插入TF树并发布

/*
Node Function:
create tf data , calculate and publish velocity command to turtle2
*/
#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>

std::string turtle_name;
//回调函数
void PoseCallBackFunction(const turtlesim::PoseConstPtr& msg)
{
	//create TF_Broadcaster
	//广播器
	static tf::TransformBroadcaster br;
	
	//TF_data INIT
	//设置turtle相对于world的坐标变换
	tf::Transform transform;
	//setOrigin设置平移变换
	transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));
	tf::Quaternion qt;
	qt.setRPY(0,0,msg->theta);
	//setOrigin设置旋转变换
	transform.setRotation(qt);
	
	//Broadcast TFData between world and turtle Coordinate System
	//发布坐标变换
	//该类型的TF消息不仅包含tf::Transform的坐标变换与时间轴，而且要指定父坐标系和子坐标系
	br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));	
}

//char** argv is used to figure out turtle1 and turtle2
//but the Node name should be unique and only
//how to set different Node name for turtle1 and turtle2
//it's called Remap
int main(int argc,char** argv)
{
	//ROSINIT
	ros::init(argc,argv,"my_tf_broadcaster");
	
	//INIT NodeHandle
	ros::NodeHandle n;
	
	//name the turtle
	if(argc!=2)
	{
		ROS_ERROR("Need Turtle Name as Argument");
		return -1;
	}
	
	turtle_name= argv[1];
	
	//Create a subscriber for Pose of turtle
	//turtle_name could be turtle12345678
	ros::Subscriber sub=n.subscribe(turtle_name+"/pose",10,&PoseCallBackFunction);
	
	//Waiting for CBF
	ros::spin();
	
	return 0;
}

五、创建TF监听器 TF Listener

turtle1相对于world的TF信息已经插入树，Listener从TF树处监听TF消息，并从中获取turtle2关于turtle1的坐标变换，从而指导turtle2的运动

/*
Node Function:
Listen to TFDATA,calculate and pub turtle2 velocity command
*/
#include<ros/ros.h>
#include<tf/transform_listener.h>
#include<geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc,char** argv)
{
	//ROSINIT
	ros::init(argc,argv,"my_tf_listener");
	ros::NodeHandle n;
	
	//request service /spawn to create turtle2
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle =n.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);
	
	//Create velocity command Publisher for turtle2
	ros::Publisher turtle_vel=n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);
	
	//Create TF Listener
	tf::TransformListener listener;
	ros::Rate rate(10.0);//监听TF树，缓存10.0s
	while(n.ok())
	{
		//get TF Data between turtle1 and turtle2 Coordinate System
		tf::StampedTransform transform;
		//Duration :3s overtime
		//lookup relation between turtle1 and turtle2
		//result saves in Variable transform
		try
		{
			listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
			listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);
			//获取turtle2和turtle1的变换关系并存在transform里
		}
		catch(tf::TransformException &ex)
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		
		//pub turtle2 velocity command based on relationship between t1 and t2 Coordinate System
		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;
}

六、配置launch文件

<launch>
<!-- turtlesim Node -->
	<node
		pkg="turtlesim"
		type="turtlesim_node"
		name="sim_node"
	/>
<!-- turtlesim keyboard Node -->	
	<node
		pkg="turtlesim"
		type="turtle_teleop_key"
		name="keyboard_node"
	/>
<!-- turtle1 and turtle2 TF Broadcaster Node -->
	<node
		pkg="learning_tf"
		type="turtle_tf_broadcaster"
		name="Turtle1_TF_Broadcaster"
		args="/turtle1"
	/>

	<node
		pkg="learning_tf"
		type="turtle_tf_broadcaster"
		name="Turtle2_TF_Broadcaster"
		args="/turtle2"
	/>
<!-- TF Listener for controling  velocity of turtle2-->
	<node
		pkg="learning_tf"
		type="turtle_tf_listener"
		name="Turtle_Listener"
	/>
</launch>