【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广播和监听的程序编写了

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值