フレームを追加する(C++)

今日は大晦日のROS日和ですからじゃんじゃんいきましょう。

フレームを追加する（C++)
http://www.ros.org/wiki/tf/Tutorials/Adding%20a%20frame%20(C%2B%2B)

ローカル座標系の連結がテーマです。
この重要性はロボティクスを勉強した人なら分かりますね。
tfがやりたいのはまさに今回やるテーマだと思います。

例として、レーザーレンジファインダの取り付け位置の話がでています。

レーザー位置はロボットが動くと自動的についてきます。

レーザーレンジファインダが返す、レーザーレンジファインダからの相対位置での障害物位置を、ロボット座標系に変換する、なんてことはやりそうですよね。
tfを使えばそういう変換が楽勝にできます。

tfではツリー構造を作ります。親１つ、子多数の関係です。
循環構造は作れません。
今回作るツリー構造は以下の図です。
turtle1の子としてcarrot1を作ります。
今まで作った/turtle1, /turtle2（引数で指定した）は/worldの子でしたね。

ではいつものように始めます。


 $ roscd learning_tf

していつものところへ。そして以下をsrc/frame_tf_broadcaster.cppとして保存しましょう。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0) );
    br.sendTransform(transform, ros::Time::now(), "carrot1", "turtle1");
    rate.sleep();
  }
  return 0;
};

以前作ったbroadcasterと似たコードが並んでいますね。
違うのはコールバックでないこと、と以下が違います。

br.sendTransform(transform, ros::Time::now(), "carrot1", "turtle1");

最後のsendTransformで、turtle1を親にして、carrot1という名前で新しいフレームを作っています。しかし、この引数をとるsendTransformはdepricatedなので、以下のように書き換えたほうがいいですね。

br.sendTransform(tf::StanpedTransform(transform, ros::Time::now(), "turtle1", "carrot1");

その上のところはオフセットです。turtle1からY方向に2.0だけオフセットした位置にcarrot1を作っています。ところで、亀なのにcarrotを追いかけるってどうなの・・・。

transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0) );

ではいつものようにビルドしてください。

CMakeLists.txtに以下を追加。

rosbuild_add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)

そして、

$ make

です。

そしてlaunchファイルも追加しましょう。

<launch>
    ...
    <node pkg="learning_tf" type="frame_tf_broadcaster"
          name="broadcaster_frame" />
  </launch>

です。普通に起動するだけですね。

で、普通に起動します。

 $ roslaunch learning_tf start_demo.launch

あれ？前回と何も変わりません。

じゃあ、前回作ったsrc/turtle_tf_listener.cppを編集して、carrotを追いかけるようにしましょう。/turtle1を/carrot1に置き換えるだけです。

listener.lookupTransform("/turtle2", "/carrot1",
                           ros::Time(0), transform);

で、make

して、launchしましょう。

しっかり亀の左によりそう、いい亀になりましたね。

最後におまけで、動くフレームを作っててみましょう。

src/frame_tf_broadcaster.cppを以下のように変更します。

transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0) );

亀のまわりをぐるぐる回る亀ができましたとさ。

今回はこれでおしまいです。