零.前言
关于tf的内容,我也没有彻底理解透彻,只能讲述个大概,若有错误,请评论区斧正,谢谢。
在《机器人基础》这个课里,有推导关于机器人坐标变换的说明,当然,我没学,学了也因为当时数学不好没听懂,所以这篇文章也不会讲太多数学层面的东西。
一.概念
1.1 引言
我们知道,物体的位置是相对的。
那么在描述一个系统中例如三个物体的位置关系,我们可以通过假设A为原点,计算B相对于A的坐标,C相对于A的坐标。这样可以通过A+坐标的方式表示B,C的位置。
当然,此时我们也可以把B当作坐标原点,根据刚才的坐标,我们可以得到反推得到A相对于B的坐标,又已知AC间的坐标关系,那么就可以通过代数计算得到C相对于B的坐标。
ROS中的坐标也是如此!
1.2 思想
ROS中,所有的坐标关系是通过一颗树形结构组成的,就像这样:
这样做的好处是:假设A为我们认为的参照物,把它看作为原点,那么BD一起向某个方向移动了相同的距离。那么我们用绝对坐标的方式需要会改变B和D这两个物体的坐标。
但是通过这种树形结构的方式,我们只需要记住D和B的相对位置,那么我们只需要更改B相对于A的位置,在需要时,即可计算出D的位置,这样可以有效地简化坐标系变化时的坐标变动的数目,每个目标只用专心处理自己和目标坐标系的关系,不用考虑其他坐标系的关系。
1.3 概念
两个坐标系间的tf消息组成是这样的:
header:
seq: 0
stamp:
secs: 1621956538
nsecs: 108681917
frame_id: "parent"
child_frame_id: "child"
transform:
translation:
x: -0.45
y: 0.2
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
描述的是从frame_id
变换到child_frame_id
的坐标关系。很多很多这样的消息就可以构成一棵tf消息树。完成整个系统的坐标描述
上面还有roscore运行的时间戳,能够记录一段时间的实时坐标消息。
1.4 使用
那么我们需要做的:
- 广播tf变换
- 监听tf变换
通过广播,把变动的两个坐标系的关系发布出去,然后由ros底层提供处理,然后并通过话题发布出来,然后需要用到坐标系时,监听某两个坐标系间的关系即可(当然,这个关系可能不是直接得出,得经过计算)。不过,复杂的计算和其他问题的处理我们不需要关心。我们只需要发布,监听即可。
1.5 特点
优点:
- 各种数值计算的细节,你不用考虑,tf库可以帮你
- 接口很简洁,会广播和监听就行;
- 问题找的很准,那就是需要维护坐标系之间的关系,尤其是父子坐标系的关系
- 提供了很多工具程序
- 考虑了与时间相关的变换
缺点:
- 树的结构很简单,但有时候很笨拙。对于同级的坐标系,就需要从下到上找共同先辈,然后从这个先辈再往下找,进而确定二者的关系。
- 每个订阅器要想获得某两个坐标系的关系都要搜索同一颗树,这样的开销太大,主要是网络传输的负荷比较大。
- 很难满足实时性的要求,这一点比较显然。这也是为什么TF会将每个变换存10秒钟的数据
- 虽然整体比较容易上手但是很多细节不易理解。比如,now()和time(0);比如,技术文档里的一些术语名词;比如,采用了机器人里的习惯,与飞行器,惯导,车辆里的习惯区别较大,使用时不能想当然。
二.代码实现
2.1 广播程序
header:
seq: 0
stamp:
secs: 1621956538
nsecs: 108681917
frame_id: "parent"
child_frame_id: "child"
transform:
translation:
x: -0.45
y: 0.2
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
以上是一个tf消息,我们分析该消息可以得到:我们可能需要修改以下内容的值:stamp
frame_id
child_frame_id
translation:
rotation:
通过查阅官方手册,我们可以得知:
-
br.sendTransform();
该函数可以发送我们的tf变换的消息 -
tf::StampedTransform(transform, ros::Time::now(), child_frame_id, frame_id);
该函数可以创建一条我们需要发送的tf消息。他会自动更根据ros::Time::now()
的时间,为我们添加stamp
这样的时间戳。 -
transform
是一个类,它包含了translation
、rotation
这两个tf需要用到的元素。 -
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) )
可以修改transform.translation
的值,参数为相对于父坐标系的x,y,z -
transform.setRotation( tf::Quaternion(0, 0, 0, 1) )
可以修改transform.rotation
的值,参数为我们学过的2X2的旋转矩阵。 -
tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q);
但是手动计算这个旋转矩阵太麻烦了,所以我们可以通过创建Quaternion对象,并通过rpy
作为参数,实例化该对象,它会自动根据rpy
的三个参数创建旋转矩阵。
整个结构像这样:
至此,我们可以通过x,y,z,r,p,y这六个我们认识的参数来创建并广播我们的tf消息了。
2.2 监听程序
监听程序很简单:listener.lookupTransform(child_frame_id, frame_id, ros::Time(0), transform);
- 参数除了第三个ros::Time(0),其余的都在广播程序里讲过。
-
ros::Time(0)
代表 (tf树由后台计算处理) 最新可用的tf树的时间。若此处使用
整个ROS的时间系统是相对于独立的,使用的是ros的内部时间,所以要实现延迟跟随,得用该方法获得x秒前的时间ros::Time::now(); ros::Time past = now - ros::Duration(x);
三.实战——多海龟跟随
3.1 创建功能包
现在忽略掉工作空间、工作空间的环境配置。直接创建功能包:
catkin_create_pkg turtle_follow roscpp geometry_msgs tf turtlesim
3.2 创建乌龟发布节点
生成海龟的原理:是向Spwan
这个服务提供一个请求,这个过程可以参考我的
这是海龟生成的服务Spwan.srv
float32 x
float32 y
float32 theta
string name # Optional. A unique name will be created and returned if this is empty
---
string name
那么根据服务编程的原则,我们可以写出以下代码:
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
int i;
if (argc < 2)
{
ROS_ERROR("Turtle's name can't be empty");
return -1;
}
ros::init(argc, argv, "turtle_new");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv_msg;
for (i = 1; i < argc; ++i)
{
srv_msg.request.x = i;
srv_msg.request.y = i;
srv_msg.request.theta = 0;
srv_msg.request.name = *(argv + i);
add_turtle_client.call(srv_msg);
}
return 0;
}
解释一下:
引用的库
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
检测调用该节点是否含有参数,若没有,则报警并退出:
if (argc < 2)
{
ROS_ERROR("Turtle's name can't be empty");
return -1;
}
初始化节点:
ros::init(argc, argv, "turtle_new");
ros::NodeHandle node;
等待spawn
服务创建(因为roslaunch里启动不分顺序,在这里必须等待,确保该服务存在),然后初始化一个客户端,并创建一条srv_msg
服务的数据结构,准备访问服务:
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv_msg;
通过循环,更改乌龟的初始坐标和名字,然后进行访问。(乌龟在二维平面上运动,所以只有x轴,y轴,和yaw角)
for (i = 1; i < argc; ++i)
{
srv_msg.request.x = i;
srv_msg.request.y = i;
srv_msg.request.theta = 0;
srv_msg.request.name = *(argv + i);
add_turtle_client.call(srv_msg);
}
将上面的总体代码创建为turtle_new.cpp
添加CMakeList
:
add_executable(turtle_new src/turtle_new.cpp)
target_link_libraries(turtle_new ${catkin_LIBRARIES})
编译,然后在三个终端里按顺序
运行:
roscore
rosrun turtlesim turtlesim_node
rosrun turtle_follow turtle_new A B C D E
得到结果:
然后查看话题,成功创建五只乌龟,还有自带的turtle1
,有强迫症的同学,可以通过编写kill
服务去杀死那只turtle1
。
3.2 创建坐标广播器
首先我们先看一下乌龟的姿态信息:rostopic echo /A/pose
x: 1.0
y: 1.0
theta: 0.0
linear_velocity: 0.0
angular_velocity: 0.0
---
这是我们创建时,相对于world
这个坐标系的相对坐标:xy轴,y角,线速度,角速度。
所以,我们要将该/pose
话题提供的信息,转换成tf消息并发出。代码如下:
#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 transform;
tf::Quaternion quaternion;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
quaternion.setRPY(0, 0, msg->theta);
transform.setRotation(quaternion);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "turtle_broadcaster");
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = *(argv + 1); // = argv[1]
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
解释一下:
主函数里要求程序要有一个参数,也就是乌龟的名字。然后就是基础的节点编程的原理,乌龟发布的位置的话题是:/乌龟的名字/pose
,我们需要订阅该话题,该话题发布后,我们就进入回调函数,并广播tf消息。
int main(int argc, char** argv)
{
ros::init(argc, argv, "turtle_broadcaster");
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = *(argv + 1); // = argv[1]
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
回调函数里,创建了 一个广播器br
,姿态信息transorm
,和旋转矩阵quaternion
。
在2.1里详细的描写了该过程,就不赘述。
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion quaternion;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
quaternion.setRPY(0, 0, msg->theta);
transform.setRotation(quaternion);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
将我们的代码写入turtle_broadcaster.cpp
:
添加依赖:
add_executable(turtle_broadcaster src/turtle_broadcaster.cpp)
target_link_libraries(turtle_broadcaster ${catkin_LIBRARIES})
编译运行:
roscore
rosrun turtlesim turtlesim_node
rosrun turtle_follow turtle_new A
rosrun turtle_follow turtle_broadcaster A
此时我们可以通过:rostopic echo /tf
查看我们的tf树,或者使用rosrun tf tf_echo [reference_frame] [target_frame]
查看任意两个坐标系的变换,或者使用rosrun rqt_tf_tree rqt_tf_tree
在rqt里可视化查看:
3.3 创建坐标监听器,并发布速度话题
先rostopic echo /turtle1/cmd_vel
看看/cmd_vel
这个话题是啥样的:
无论怎么移动小乌龟,他只有x,z轴数据在变,所以我也懒得找文献是为什么了,直接给这两个速度赋值就行。
那么我们监听器的代码是:
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <string>
using namespace std;
int main(int argc, char** argv)
{
ros::init(argc, argv, "turtle_listener");
ros::NodeHandle node;
string follower_vel("/");
if (argc != 3)
{
ROS_ERROR("Need two names of the turtles!");
return -1;
}
follower_vel.append(argv[1]);
follower_vel.append("/cmd_vel");
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(follower_vel, 10);
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
tf::StampedTransform transform;
try
{
listener.lookupTransform(argv[1], argv[2], ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;
}
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;
};
目前我们坐标系的命名因该是这样的:world
、A
……
速度消息发布的话题是这样的:/A/cmd_vel
。
等会儿我们处理的时候,最好按着格式来,一个符号都不要差(虽然有些是可以省略)。
解释一下代码, 这里我们除了用到Twist这个消息,还要使用string
对字符串拼接.
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <string>
初始化节点,并且初始化我们的/X/cmd_vel
这个话题的字符串。因为是跟随,所以我们需要两个参数。
ros::init(argc, argv, "turtle_listener");
ros::NodeHandle node;
string follower_vel("/");
if (argc != 3)
{
ROS_ERROR("Need two names of the turtles!");
return -1;
}
添加字符串,完善follower_vel
这个字符串,等会儿launch只输入乌龟名即可,并创建Twist消息的发布器和tf监听器
follower_vel.append(argv[1]);
follower_vel.append("/cmd_vel");
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(follower_vel, 10);
tf::TransformListener listener;
这个是获得两个坐标系的transform
信息,用try起来是因为 本身这是个异步过错,容易出错。等会儿运行时就会看到真的会出错。
tf::StampedTransform transform;
try
{
listener.lookupTransform(argv[1], argv[2], ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;
}
创建消息,通过坐标x,y得到角度差距,并求得y轴的角速度,和欧式距离下的线速度,并发布消息。
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();
将我们的代码写入:
添加依赖:
add_executable(turtle_listener src/turtle_listener.cpp)
target_link_libraries(turtle_listener ${catkin_LIBRARIES})
这样就完成了。
3.4 编写launch文件
在功能包下创建launch
文件夹,并创建launch
文件:
<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 创建一个乌龟节点 -->
<node pkg="turtle_follow" type="turtle_new" args="A" name="turtle_new" />
<!-- 创建两个乌龟的tf广播器 -->
<node pkg="turtle_follow" type="turtle_broadcaster" args="A" name="turtle1_broadcaster" />
<node pkg="turtle_follow" type="turtle_broadcaster" args="turtle1" name="turtle2_broadcaster" />
<!-- 第一个参数的乌龟跟随第二个参数的乌龟 -->
<node pkg="turtle_follow" type="turtle_listener" args="A turtle1" name="listener1" />
</launch>
这个launch文件就不需要太多的解释了,启动节点,添加参数,不懂的话可以看上一篇文章的内容。
然后运行看效果:
roslaunch turtle_follow follow.launch
看,因为tf
坐标系还没来得及建立而产生的错误:
如果没有一直出现红色的错误信息,说明代码无误,在这个终端用方向键移动乌龟:
千辛万苦写出来的代码不来点儿好玩儿的?6只乌龟开火车!:
<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 创建五个乌龟节点 -->
<node pkg="turtle_follow" type="turtle_new" args="A B C D E" name="turtle_new" />
<!-- 创建六个乌龟的tf广播器 -->
<node pkg="turtle_follow" type="turtle_broadcaster" args="A" name="turtle1_broadcaster" />
<node pkg="turtle_follow" type="turtle_broadcaster" args="B" name="turtle2_broadcaster" />
<node pkg="turtle_follow" type="turtle_broadcaster" args="C" name="turtle3_broadcaster" />
<node pkg="turtle_follow" type="turtle_broadcaster" args="D" name="turtle4_broadcaster" />
<node pkg="turtle_follow" type="turtle_broadcaster" args="E" name="turtle5_broadcaster" />
<node pkg="turtle_follow" type="turtle_broadcaster" args="turtle1" name="turtle0_broadcaster" />
<!-- 第一个参数的乌龟跟随第二个参数的乌龟 -->
<node pkg="turtle_follow" type="turtle_listener" args="A turtle1" name="listener1" />
<node pkg="turtle_follow" type="turtle_listener" args="B A" name="listener2" />
<node pkg="turtle_follow" type="turtle_listener" args="C B" name="listener3" />
<node pkg="turtle_follow" type="turtle_listener" args="D C" name="listener4" />
<node pkg="turtle_follow" type="turtle_listener" args="E D" name="listener5" />
</launch>
四.后记
要深入学习tf,还是得多看官方文档、手册。接下来学习了gazebo后,就可以让机器人动起来了!