【ROS】学习笔记七 ubuntu16.04下vs code实现ros下tf坐标系的广播与监听编程

一、tf坐标系管理系统初识

tf坐标系主要包含广播和监听两部分，在编写这两部分程序之前，需要下载对应于自己版本的tf功能包，以turtle_tf功能包为例进行说明，运行以下几行代码，可以初步了解什么是tf坐标系管理

sudo apt-get install ros-kinetic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key
rosrun tf view_frames

可以发现，通过运行turtle_tf下的turtle_tf_demo.launch文件，产生了一个界面，初始界面中有两只海龟，其中一只海龟迅速向另一只海龟移动靠近。新建一个终端，输入命令rosrun turtlesim turtle_teleop_key后，通过键盘可以控制第一只海龟运动，另一只海龟会跟随第一只海龟运动，直至重合。输入指令rosrun tf view_frames后，会生成一个pdf文件，通过这个文件我们可以查看运行示例中所有坐标系之间的关系

另外，我们也可以通过tf_echo来查看不同坐标系之间的关系

rosrun tf tf_echo turtle1 turtle2

当然，我们也可以通过三维可视化工具rviz工具来观察他们之间的坐标系变化关系，打开rviz后需要将Fixed Frame的选项改为world，同时添加TF项，才能看到坐标图样，通过键盘可以控制小海龟的运动，同时，可以在rviz中看到同样的效果

rosrun rviz rviz -d ‘rospack find turtle_tf’ /rviz/ture_rviz.rviz

二、工作空间的创建及准备工作

本章节主要内容是以tf坐标的广播与监听编程为主，使用的IDE是vs code，关于ubuntu系统下vs code的安装可以参考我的另一篇博客，链接如下

https://blog.csdn.net/weixin_45417246/article/details/115444048.

新建文件夹catkin_tf，并在文件夹下新建src文件夹，这里可以不进行工作空间的初始化，并用vs code打开catkin_tf文件，按住Ctrl+Tab+～键，在vs code打开终端，在/fjy/catkin_tf路径下进行编译，并添加环境变量

cd ~/fjy/catkin_tf //转到catkin_turtle路径下
catkin_make //编译
source devel/setup.bash //设置环境变量

按住Fn+F1，在搜索栏中输入C/C++编辑配置，点击C/C++编辑配置，会在左边目录产生一个.vscode文件夹，单击该文件夹下的c_cpp_properties.json文件，并添加路径，按Ctrl+s保存，路径如下

“/opt/ros/kinetic/include” //使头文件包含合理

三、tf的广播与监听程序编写及运行效果

在/catkin_tf/src路径下创建功能包tf，博主这边通过终端命令行进行创建

catkin_create_pkg learning_tf roscpp tf turtlesim

然后在/fjy/catkin_tf/src/learning_tf/src路径下创建turtle_tf_broadcaster.cpp文件和turtle_tf_listener.cpp

touch turtle_tf_broadcaster.cpp
touch turtle_tf_listener.cpp

编写turtle_tf_broadcaster.cpp文件

//包含ros相关的API头文件
#include <ros/ros.h>
//包含在tf库里的transform_broadcaster消息
#include <tf/transform_broadcaster.h>
//包含在turtlesim里的Pose消息
#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);//把数据保存到transform变量中
	// 广播world与海龟坐标系之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    // 创建一个节点，初始化ROS节点
	ros::init(argc, argv, "tf_broadcaster");
	// 输入参数作为海龟的名字
	if (argc != 2)
	{
		ROS_ERROR("need turtle name as argument"); 
		return -1;
	}
	turtle_name = argv[1];
	// 订阅海龟的位姿话题
	ros::NodeHandle node;
    //创建一个订阅者sub，订阅turtle_name+“/pose“话题，队列长度为10
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    // 循环等待回调函数
	ros::spin();
	return 0;
};

编写turtle_tf_listener.cpp文件

//包含ros相关的API头文件
#include <ros/ros.h>
//包含tf库里的transform_listener消息
#include <tf/transform_listener.h>
//包含geometry_msgs库里的Twist消息
#include <geometry_msgs/Twist.h>
//包含turtlesim库里的Spawn消息
#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");//阻塞型函数，等待有服务到来
    //创建一个客服端，消息类型是turtle::Spawn，话题名是/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
		{
			//查询是否有这两个坐标系，查询当前时间，如果超过3s则报错
			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;
};

然后编辑功能包learning_tf下的CMakeLsits.txt文件和package.xml文件，package.xml文件一般在创建功能包时默认设置好了，不需要修改，只需要在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}) //连接库

完成这些后，回到/catkin_parameter路径下，进行编译

编译完成后，输入以下指令，观察运行效果

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_brocaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_brocaster /turtle2
rosrun learning_tf turtle_tf_listener

这样，我们就完成了vs code下tf广播和监听的程序编写了