【ROS入门】12. ROS中tf坐标系广播与监听的编程实现

一. 工作空间

1. 创建工作空间

创建的步骤可以看前面的文章，工作空间命名为sor_ws 【ROS入门】3. 创建工作区间与功能包

2.创建功能包

复制代码

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

learning_tf目录下生成以下文件及文件夹

二、实现tf广播器

首先创建turtle_tf_broadcaster.cpp文件

复制代码

cd ~/sor_ws/src/learning_tf/src
touch turtle_tf_broadcaster.cpp

实现一个tf广播器的步骤：

定义TF广播器
创建坐标变换值

发布坐标变换

复制代码

 /***********************************************************************
 Copyright 2020 GuYueHome (www.guyuehome.com).
 ***********************************************************************/
 
 /**
  * 该例程产生tf数据，并计算、发布turtle2的速度指令
  */
 
 #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;
 };

首先程序要注意头文件的书写，能够是海龟正常运行，不管是海龟turtle1或海龟turtle2与word坐标系之间的位置关系都是通用的程序，在程序中通过输入参数确定当前小海龟的名称，并且去跟word坐标系去建立关系，因此程序会运行两遍，但是我们需要注意的是，任意的一个ros节点它的节点名在整个ros环境中只能有一个，如果程序运行两遍，会因为节点名字一样冲突，因此用到了重映射。后面会详细说明

程序中将创建一个tf广播器，初始化tf数据，广播word与海龟坐标系之间的tf数据时需要设置一个时间戳(一般为１０秒)。

三.实现tf监听器

首先创建turtle_tf_listener.cpp文件

复制代码

cd ~/sor_ws/src/learning_tf/src
touch turtle_tf_listener.cpp

实现一个TF监听器的步骤

定义TF监听器

查找坐标变换

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**

该例程监听tf数据，并计算、发布turtle2的速度指令
*/

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_listener");

复制代码

  // 创建节点句柄
  ros::NodeHandle node;

  // 请求产生turtle2
  ros::service::waitForService("/spawn");
  ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

  // 创建发布turtle2速度控制指令的发布者
  ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

  // 创建tf的监听器
  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok())
  {
  	// 获取turtle1与turtle2坐标系之间的tf数据
  	tf::StampedTransform transform;
  	try
  	{
  		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;
  	}

  	// 根据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;

};

通过tf监听器去监听turtle1和turtle2之间的坐标系关系，根据turtle１与turtle2坐标之间的关系，发布turtle2的速度控制指令。

四.配置编译规则

在learning_tf文件夹下双击打开CMakeLists.txt文件:

复制代码

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})

五.运行

1.编译

复制代码

cd ~/sor_ws
catkin_make

编译完成。

2.运行

（1）打开新终端，输入roscore

（2）再打开一个新终端，启动可视化海龟界面：

复制代码

rosrun turtlesim turtlesim_node

（3）发布海龟1与world的坐标系关系：

复制代码

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

（4）发布海龟2与world的坐标系关系：

复制代码

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

（5）创建海龟2发布速度指令然后让它跟着海龟1运动：

复制代码

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

（6）运行指令，用键盘上的上下左右按键来控制海龟1运动，海龟2会跟随海龟1运动：

复制代码

rosrun turtlesim turtle_teleop_key