tf listenerを作る(C++)

今回はbroadcastされたtfを読み取るtf listenerをC++で作ります。

前回作ったlearning_tfパッケージで作業しましょう。
$ roscd learning_tf

例のごとく以下をsrc/turtle_tf_listener.cppとして保存しましょう。

#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");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

  ros::Publisher turtle_vel = 
       node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);

  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/turtle2", "/turtle1",  
                               ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
    }

    turtlesim::Velocity vel_msg;
    vel_msg.angular = 4 * atan2(transform.getOrigin().y(),
                                transform.getOrigin().x());
    vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                 pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};

では一行ずつ見ていきます。

 #include <tf/transform_listener.h>

TransformListenerクラスを使って処理を楽するためにインクルードします。

tf::TransformListener listener;

そしてこれでインスタンスを生成しました。
このインスタンスは生成されると自動的にtfのデータを受信しはじめ、１０秒間tfのデータをバッファします。

try{
      listener.lookupTransform("/turtle2", "/turtle1",  
                               ros::Time(0), transform);
    }

これは
/turtle2から見た/turtle1の座標系を、最新の時間(ros::Time(0))のもので、transformにセットしろという意味です。ros::Time(0)は最新の時間を表すようです。
見つからない時に例外を発生させるのでtryでかこんでます。

チュートリアルでは解説されていませんが、他に、spawnサービスを使って、亀を１匹生成しています。

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<turtlesim::Velocity>("turtle2/command_velocity", 10);

以下の追従制御を行っています。この入力としてturtle1,turtle2のposeを直接取得するのではなく、tfからの座標変換結果を用いています。broadcasterがいるのでこのように変換が簡単にできます。

turtlesim::Velocity vel_msg;
    vel_msg.angular = 4 * atan2(transform.getOrigin().y(),
                                transform.getOrigin().x());
    vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                 pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);

では、いつものようにビルドします。CMakeLists.txtに以下を追加し、

rosbuild_add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)

$make

です。

また、前回作ったstart_demo.launchに以下を追加します。

<launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener" 
          name="listener" />
  </launch>

そして以下のようにして起動しましょう。

 $ roslaunch learning_tf start_demo.launch

ストーカー亀ができたでしょうか？

以下のようなエラーがでてると思いますが、気にしなくていいです。

亀２(turtle2）のbroadcasterが動く前にlistenerが動いているため、turtle2が見つからないぞ、というエラーがでます。try-catchでちゃんとエラーが捕まえられていますね。せっかくなので、速度指令値をいれるところもtryの中にいれたほうがいい気がしますね。

[ERROR] 1253915565.300572000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.
[ERROR] 1253915565.401172000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.

今回は以上です。