ROS之tf

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>

rosrun tf tf_monitor

rosrun tf tf_echo /map /odom

这两个命令可以帮助查看tf当前的状态跟关系.

下面这个命令可以将tf关系输出pdf文件:

rosrun tf view_frames