tf, 很神奇的一个东西...想象一个使用场景, 一个小车, 下面有几个轮子, 中间是个底盘, 上面有若干激光传感器/雷达传感器/接近开关, 为了在运动的时候, 把这些底盘上的东西跟着底盘一起运动, 使用tf就能解决这样的问题.
ok, 如果是固定在小车的一般东西, 倒也没啥, 如果小车上, 还有一个机械臂, 小车在前进的同时, 机械臂做圆周运动, 那么机械臂在物理空间(所谓world或者map)的运动, 就必须通过tf来简化跟实现了.
首先, 看看一个tf监听者的代码:
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Velocity.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
//初始化
ros::NodeHandle node;
//获得节点句柄
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
//获得"spawn"服务客户端
turtlesim::Spawn srv;
add_turtle.call(srv);
//开启服务
ros::Publisher turtle_vel = node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
//广告一个矢量消息, 名字是"turtle2/command_velocity"
tf::TransformListener listener;
//new一个监听者.
ros::Rate rate(10.0);
//一秒刷新10次
while (node.ok()){
tf::StampedTransform transform;
//new一下带时间戳的tf
try{
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
//获取父节点的/turtle2, 子节点/turtle1的tf
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
turtlesim::Velocity vel_msg;
//new一个矢量信息, 通常包括xyz跟rpy
vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
//将龟1的广播出来的角度信息, 做一个转换
vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
//将龟1广播出来的线向信息, 做一个转换
turtle_vel.publish(vel_msg);
//把这个消息广播出去.
rate.sleep();
}
return 0;
};
这个监听者基本上就是获取广播者(下面的例子)广播出来的龟1的tf信息做了一个转换, 然后通过topic publish出来: "turtle2/command_velocity"
广播者broadcaster:
#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;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
//将消息里面的x,y组成矢量类型数据, 扔进tf
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
//写入tf的rpy部分
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
//将tf组成待时间戳的StampedTransform格式发布出去,父frame叫world, 自己就叫龟1/龟2
}
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];
//获取输入形参当龟名
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
//订阅者龟/pose的topic
ros::spin();
return 0;
};
将两个龟节点, 都启动, 再加入一个控制龟1的运动的节点, 做成一个launch:
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<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" />
<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />
<node pkg="learning_tf" type="frame_tf_broadcaster" name="broadcaster_frame" />
</launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<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" />
<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />
<node pkg="learning_tf" type="frame_tf_broadcaster" name="broadcaster_frame" />
</launch>
rosrun tf tf_monitor
rosrun tf tf_echo /map /odom
这两个命令可以帮助查看tf当前的状态跟关系.
下面这个命令可以将tf关系输出pdf文件:
rosrun tf view_frames