目录

一、TF功能包的作用

二、海龟案例

1.安装海龟的tf功能包

2.启动tf脚本文件

 3.启动海龟运动控制

三、通过TF查看坐标系关系

1.命令行工具

1）可视化查看坐标系关系

2）.直接查询树中两个坐标系关系

2.可视化工具

四、广播者和监听者

五、重映射

一、TF功能包的作用

1. 管理所有坐标系，通过查询可以获得不同坐标系之间的相互关系。
2. 具有时间属性，可以记录10s内机器人所有坐标系的相互关系
3. 底层实现采用的是一种树状数据结构，通过树可以查询坐标系
4. 可以在任意时间，将点、向量等数据的坐标进行坐标变换

二、海龟案例

目的：创建两种海龟，操控一只海龟，另一只海龟自动跟随。

1.安装海龟的tf功能包

sudo apt-get install ros-melodic-turtle-tf

2.启动tf脚本文件

roslaunch turtle_tf turtle_tf_demo.launch

 3.启动海龟运动控制

rosrun turtlesim turtle_teleop_key

三、通过TF查看坐标系关系

1.命令行工具

1）可视化查看坐标系关系

rosrun tf view_frames

注：是生成一个pdf文件，默认保存在主目录中

2）.直接查询树中两个坐标系关系

rosrun tf tf_echo 坐标系1 坐标系2

eg：

rosrun tf tf_echo turtle1 turtle2

2.可视化工具

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

1）将坐标系改成全局坐标系

2）添加TF

四、广播者和监听者

ros调用tf功能包进行广播与监听https://download.csdn.net/download/m0_56451176/86398722?spm=1001.2014.3001.5503 

1.创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

2.广播者

cd ~/catkin_ws/src/learning_tf/src
touch turtle_tf_broadcaster.cpp
gedit turtle_tf_broadcaster.cpp

turtle_tf_broadcaster.cpp代码如下：

/*
该例程产生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)
{
	//定义一个广播，利用它发布坐标系转换关系话题
	static tf::TransformBroadcaster br;
	
	//初始化tf数据
    //定义存Transform信息: 旋转和平移
	tf::Transform transform;

    //设置坐标原点,setOrigin()函数的参数类型需要为tf::Vector3类型
	transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));

    // 设置四元数
	tf::Quaternion q;
    //setRPY()函数的参数为”turtle1”在“world”坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴)
	q.setRPY(0, 0, msg->theta);
	transform.setRotation(q);
    
	//广播world与海龟坐标之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
    //transform:存储变换关系的变量
    //ros::Time::now():广播tf使的时间戳
    //“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;
}

3.监听者

cd ~/catkin_ws/src/learning_tf/src
touch turtle_tf_listener.cpp
gedit turtle_tf_listener.cpp

turtle_tf_listener.cpp代码如下：

#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
		{
            //这个函数的意思是从当前时刻开始查看2个海龟的tf坐标，持续4秒，如果存在就直接跳过，不存在则退出
			listener.waitForTransform("/turtle2", "turtle1", ros::Time(0), ros::Duration(4.0));
			//这个函数的意思是把当前2海龟的tf坐标保存到transfrom中
			listener.lookupTransform("/turtle2", "turtle1", ros::Time(0), transform);
			
		}
		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;
} 

4.配置CMakeLists.txt

在build区域中添加：

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

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

5.编译

cd ~/catkin_ws
catkin_make

6.运行

启动ROS核心

roscore

启动海龟仿真器 

rosrun turtlesim turtlesim_node

运行广播者 

source ~/catkin_ws/devel/setup.bash
rosrun learning_tf turtle_tf_broadcaster.cpp __name:=turtle1_tf_broadcaster /turtle1

 source ~/catkin_ws/devel/setup.bash
rosrun learning_tf turtle_tf_broadcaster.cpp __name:=turtle2_tf_broadcaster /turtle2

运行监听者 

source ~/catkin_ws/devel/setup.bash
rosrun learning_tf turtle_tf_listener

启动海龟运动控制 

rosrun turtlesim turtle_teleop_key

注：由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，所以可能出现以下错误：“turtle2” passed to lookupTransform argument target_frame does not exist.

 只需等待一会，等待他自己链接上即可,或者将监听者（turtle_tf_listener.cpp）中ros::Duration(4.0)改成ros::Duration(150.0)；

listener.waitForTransform("/turtle2", "turtle1", ros::Time(0), ros::Duration(150.0));

注：从当前时刻开始查看2个海龟的tf坐标，持续4秒改成持续150秒

五、重映射

rosrun learning_tf turtle_tf_broadcaster.cpp __name:=turtle1_tf_broadcaster /turtle1

 rosrun learning_tf turtle_tf_broadcaster.cpp __name:=turtle2_tf_broadcaster /turtle2

这两个命令行中的“__name:=”实现重映射功能，使一个代码可以使多个不同节点实现同样功能，不用重复编程。