ROS中的坐标系管理系统

TF功能包的作用

tf工具包，底层实现采用的是一种树状数据结构，根据时间缓冲并维护多个参考系之间的坐标变换关系，可以帮助程序员在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换,默认记录10S内的坐标。

TF坐标变换的实现方式

• 广播TF变换
• 监听TF变换

查看TF树

rosrun rqt_tf_tree rqt_tf_tree

TF坐标系广播和监听的编程实现

实现一个海龟跟随另一个海龟

1. 创建名为learning_tf的功能包

C++

1.代码

广播者

/*
该例程产生tf数据，并计算，发布turtle的速度指令
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	//创建tf的广播器
	static tf::TransformBroadcaster br;
	
	//初始化tf数据
	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);
	//广播world与海龟坐标之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char **argv)
{
	//初始化ros节点
	ros::init(argc, argv, "my_tf_broadcaster");
	//输入参数作为海龟的名字
	if(argc != 2)
	{
		ROS_ERROR("need turtle name as argument");
		return -1;
	}
	turtle_name = argv[1];
	//订阅海龟的位置话题
	ros::NodeHandle node;
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
	//循环等待回调函数
	ros::spin();
	return 0;
}

监听者

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <cmath>
int main(int argc, char**argv)
{
	//初始化ros节点
	ros::init(argc, argv, "my_tf_listener");
	//创建节点句柄
	ros::NodeHandle n;
	//请求产生turtle2产生
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);
	//创建发布turtle2速度控制指令的发布者
	ros::Publisher turtle_vel = n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
	//创建tf的监听者
	tf::TransformListener listener;
	ros::Rate rate(10.0);
	while(n.ok())
	{
		//获取turtle1 and turtle2 坐标系之间的tf数据
		tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/turtle2", "turtle1", ros::Time(0), ros::Duration(3.0));
			//这个函数的意思是从当前时刻开始查看2个海龟的tf坐标，持续3秒，如果存在就直接跳过，不存在则退出
			listener.lookupTransform("/turtle2", "turtle1", ros::Time(0), transform);
			//这个函数的意思是把当前2海龟的tf坐标保存到transfrom中
		}
		catch(tf::TransformException &ex)
		{
			ROS_ERROR("%s", ex.what());
			ros::Duration(1.0).sleep();
			continue;
			
		}
		//根据turtle1与turtle2坐标之间的位置关系，发布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;
} 

2. 添加编译信息到MakeLists文件

3. 回到工作空间进行编译

catkin_make

4. 修改环境变量

source devel/setup.bash

5.运行（用到重映射）

roscore启动master
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster turtle1
这样节点的名字就变成了turtle1_tf_broadcast, 需要输入一个参数turtle1作为海龟的名字
执行一上指令就创立了turtle1和world的坐标关系，并且广播出去

rosrun learning_tf turtle_tf_listener
这个代码生成监听者接收广播，并且根据广播生成一个海龟2，跟随海龟1

python

1. 代码

1. 广播器

#!/usr/bin/env python3
#-*- coding:utf-8 -*-
import roslib
roslib.load_manifest("learning_tf")
import rospy
import tf
import turtlesim.msg
def handle_turtle_pose(msg, turtlename):
	br = tf.TransformBroadcaster()
	br.sendTransform((msg.x, msg.y, 0),
					tf.transformations.quaternion_from_euler(0, 0, msg.theta),
					rospy.Time.now(),
					turtlename,
					"world")
if __name__ == "__main__":
	rospy.init_node("turtle_tf_broadcaster")
	turtlename = rospy.get_param('~turtle')
	rospy.Subscriber("%s/pose" %turtlename, 
					turtlesim.msg.Pose,
					handle_turtle_pose, 
					turtlename)
	rospy.spin()

2. 监听者代码

2. 修改环境变量

source devel/setup.bash

3. python代码运行重映射的方法

发布者需要运行2个小海龟，用一个代码的直接执行只有一个节点，所以通过重映射的方式改变节点的名字

完整的运行过程：

• 运行roscero
• 运行广播器1：广播world和海龟1的位置关系

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=turtle1

• 运行广播器2：广播world和海龟2的位置关系

 rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster _turtle:=turtle2

• 运行小海龟

rosrun turtlesim turtlesim_node

• 运行监听者，生成海龟2，监听海龟1和海龟2的位置关系，同时发布速度指令让海龟2向海龟1运动

rosrun learning_tf turtle_tf_listener.py

运行结果：

生成的新海龟向第一个海龟运行

总结

• 重映射
  
• 建立广播者
  1. 定义TF广播器（TransformBroadcaster）
  2. 创建坐标变换值
  3. 发布坐标变换值