当前位置:网站首页>ROS学习(22)TF变换
ROS学习(22)TF变换
2022-07-06 18:12:00 【敲代码的雪糕】
文章目录
前言
机器人本地和机器人的工作环境中存在大量的组件元素,在机器人设计和应用中都会涉及不同组件的位置和姿态,这就需要引入坐标系以及坐标变换的概念。
坐标变换是机器人系统中常用的基础功能,ROS中的坐标变换系统由TF功能包维护。
一、TF功能包
TF是一个让用户随时间跟踪多个坐标系的功能包,使用树形数据结构,根据时间缓冲并维护多个坐标系之间的坐标变换关系,帮助开发者在任意时间、在坐标系间完成点、向量等坐标的变换。
TF可以在分布式系统中进行操作,一个机器人系统中所有的坐标变换关系,对于所有的节点组件都是可用的,所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据,所以这种结构不需要中心服务器来存储任何数据。
想要使用TF功能包,总体来说需要两步:
- 监听TF变换
接收并缓存系统中发布的所有坐标变换数据,并从中查询所需要的坐标变换关系。 - 广播TF变换
向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播,每个广播都可以直接将坐标系变换关系插入TF树中,不需要再进行同步。
二、TF工具
坐标系统涉及多个空间之间的变换,不容易进行抽象,所以TF提供了丰富的终端工具来帮助开发者调试和创建TF变换。
1、tf_monitor
1)用于打印TF树中所有坐标系的发布状态
rosrun tf tf_monitor
2)查看指定坐标系之间的发布状态
rosrun tf tf_monitor <source_frame> <target_frame>
2、tf_echo
用于查看指定坐标系之间的变换关系
rosrun tf tf_echo <source_frame> <target_frame>
3、static_transform_publisher
用于发布两个坐标系之间的静态坐标变换,这两个坐标系不发生相对位置变化。该工具需要设置坐标的偏移参数和旋转参数,发布频率以ms为单位。
命令有两种格式,如下:
1)旋转参数使用以弧度为单位的yaw/pitch/roll角度
rosrun tf static_transform_publisher x y x yaw pitch roll frame_id child_frame_id period_in_ms
2)旋转参数使用四元数
rosrun tf static_transform_publisher x y x qx qy qw frame_id child_frame_id period_in_ms
该命令还可以在launch文件中使用,如下:
<launch>
<node pkg="tf" type="static_transform_publisher" name="link_broadcaster" args="1 0 0 0 0 0 1 link_parent link 100" />
<launch>
4、view_frames
view_frames是可视化的调试工具,可以生成pdf文件,显示TF树的信息。命令如下:
rosrun tf view_frames
查看pdf文件,可以使用如下命令:
evince frames.pdf
三、乌龟例程中的TF
主要用于理解TF的作用,并且熟悉上述TF工具的使用,功能包名为turtle_tf,功能包安装命令如下:
sudo apt-get install ros-kinetic-turtle-tf
运行turtle_tf功能包,命令如下:
roslaunch turtle_tf turtle_tf_demo.launch
打开键盘控制节点,命令如下:
rosrun turtlesim turtle_teleop_key
效果如下:
可以发现,出现了两只乌龟,使用键盘方向键控制一只乌龟移动,会发现另一只乌龟会跟随移动。
其TF树如下:
如上所示,当前系统中存在三个坐标系,时间坐标系world、乌龟坐标系turtle1和乌龟坐标系turtle2。
世界坐标系是该系统的基础坐标系,其它坐标系都相对该坐标系建立,所以world是TF树的根节点,而两只乌龟坐标系的原点就是乌龟在世界坐标系下的坐标位置。
可以通过如下命令,查看两只乌龟坐标系之间的变换关系:
rosrun tf tf_monitor turtle1 turtle2
效果如下:
四、乌龟跟随例程代码实现
现在要让turtle2跟随turtle1运动,等价于turtle2坐标系向turtle1坐标系移动,首先新建learning_tf功能包。
1、创建TF广播器
创建一个节点,主要用于发布乌龟坐标系与世界坐标系之间TF变换,turtle_tf_broadcaster.cpp内容如下:
#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::Transform transform;
//设置平移变换
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
//设置旋转变换
transform.setRotation(q);
// 将坐标变换插入TF树并发布坐标变换
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化节点
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
};
turtle_name = argv[1];
// 订阅乌龟的pose信息
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
2、创建TF监听器
创建一个节点,主要用于监听TF消息,从中获取turtle2相对于turtle1的坐标系的变换,从而控制turtle2的移动。turtle_tf_listener.cpp内容如下:
#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::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;
//监听器会自动接收TF树的消息,并且缓存10秒
ros::Rate rate(10.0);
while (node.ok())
{
tf::StampedTransform transform;
try
{
// 查找turtle2与turtle1的坐标变换
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需要运动的线速度和角速度
// 并发布速度控制指令,使turtle2向turtle1移动
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;
};
3、实现乌龟跟随运动
编写start_demo_with_listener.launch文件,内容如下:
<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 两只海龟的tf广播 -->
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
<!-- 监听tf广播,并且控制turtle2移动 -->
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>
运行如下命令:
roslaunch learning_tf start_demo_with_listener.launch
效果如下:
打开键盘控制节点,命令如下:
rosrun turtlesim turtle_teleop_key
跟随效果如下:
边栏推荐
猜你喜欢
Scenario practice: quickly build wordpress blog system based on function calculation
Yunna | work order management measures, how to carry out work order management
AcWing 1148. 秘密的牛奶运输 题解(最小生成树)
AcWing 1148. Secret milk transportation problem solution (minimum spanning tree)
从底层结构开始学习FPGA----FIFO IP的定制与测试
Right mouse button customization
Analyze "C language" [advanced] paid knowledge [End]
【信号与系统】
C语言关于链表的代码看不懂?一篇文章让你拿捏二级指针并深入理解函数参数列表中传参的多种形式
Appium foundation - appium inspector positioning tool (I)
随机推荐
454-百度面经1
Baidu flying general BMN timing action positioning framework | data preparation and training guide (Part 2)
1123. 最深叶节点的最近公共祖先
新工作感悟~辞旧迎新~
shell脚本快速统计项目代码行数
制作带照明的DIY焊接排烟器
LeetCode. 剑指offer 62. 圆圈中最后剩下的数
Let's see how to realize BP neural network in Matlab toolbox
The difference between Tansig and logsig. Why does BP like to use Tansig
【信号与系统】
Mongodb checks whether the table is imported successfully
JS how to quickly create an array with length n
Yunna | work order management measures, how to carry out work order management
Set up [redis in centos7.x]
MySQL最基本的SELECT(查询)语句
AcWing 1140. Shortest network (minimum spanning tree)
Typical problems of subnet division and super network construction
When grep looks for a process, it ignores the grep process itself
JS ES5也可以创建常量?
Public key \ private SSH avoid password login