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