Note: This tutorial assumes you have completed the writing a tf listener tutorial (Python) (C++).
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

フレームを加える (C++)

Description: このチュートリアルではどのようにして特別な固定のフレームをtfに加えるかを学びます

Tutorial Level: BEGINNER

Next Tutorial: tf and time (C++)

前回のチュートリアルの中で、tf broadcasterとtf listenerを加えることでturtle のデモを再現しました。今回のチュートリアルでは、どのようにtfツリーに特別なフレームを加えるかについて学んでいきましょう。この過程はtf broadcasterを作ることにとても似ていて、tfの利便さの一端を実感できるでしょう。

なぜフレームを追加するのか

ほとんどのタスクにおいて、ローカルフレームの中について考えるのは簡単です、例えば、レーザスキャナの中心でのフレームでレーザスキャンを推論するのは簡単です。tfは、それぞれのセンサにローカルフレームを定義し、リンクし、そのほかのこともできるようにしてくれています。そして、tfは導入された特別なフレームのtransformもすべて管理してくれます。

どこにフレームを追加すべきか

tfはフレームの木構造(tree structure)を作ります(フレーム構造の中に閉ループができることが許されません)。つまり、フレームは一つの親のみを持つが、子は複数もっているということです。現在、今までのチュートリアルでできたtfツリーは3つのフレームをもっています。ワールド、turtle1、turtle2です。二つのturtleは、worldの子にあたります。もし、新しいtfを加えたいなら、3つのすでにあるフレームが親のフレームになる必要があり、新しいフレームは子フレームになるでしょう。

  • tree.png

どのようにフレームを加えるか

turtleの例を用いて、1匹目のturtleに新しいフレームを加えましょう。このフレームは2匹目のturtleにとって、"carrot"になるでしょう。

ソースファイルから作っていきましょう。まずは一つ前のチュートリアルで作ったパッケージのディレクトリに移動してください。:

 $ roscd learning_tf

コード

エディタを立ち上げ、以下のソースコードをsrc/frame_tf_broadcaster.cppとして、コピー&ペーストして保存してください。

   1 #include <ros/ros.h>
   2 #include <tf/transform_broadcaster.h>
   3 
   4 int main(int argc, char** argv){
   5   ros::init(argc, argv, "my_tf_broadcaster");
   6   ros::NodeHandle node;
   7 
   8   tf::TransformBroadcaster br;
   9   tf::Transform transform;
  10 
  11   ros::Rate rate(10.0);
  12   while (node.ok()){
  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform.setRotation( tf::Quaternion(0, 0, 0) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
  16     rate.sleep();
  17   }
  18   return 0;
  19 };

コードはtf broadcaster チュートリアルの例にとても似ています。

コード解説

このコードの中のキーとなる行を部分的に見ていきましょう。:

  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform.setRotation( tf::Quaternion(0, 0, 0) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));

ここで新しい親のturtle1から新しい子のcarrot1へのtransformを作成しています。 carrot1フレームは、turtle1から左にオフセット2メートルのところです。

frame broadcasterを実行する

コードがこれでできたので、まずコンパイルしてみましょう。CMakeLists.txtを開いて、以下の行をファイルの最後に追加してください。:

add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)
target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})

パッケージをビルドします;(catkinワークスペースのトップディレクトリにて):

 $ catkin_make

  rosbuild_add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)

パッケージをビルドします:

 $ make

ビルドが全て成功すると、binフォルダの中にturtle_tf_broadcasterと呼ばれるバイナリファイルが出来ているはずです。

このデモのためのlaunchファイル(start_demo.launch)を編集する段階に入りましょう。以下のものを<launch>タグのブロックの中にnodeブロックとしてマージします。:

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

まず、前回のチュートリアルのlaunchファイルは止めていることを確認してください(ctrl-cを使ってください)。これで、turtleのデモを始める準備ができました。

 $ roslaunch learning_tf start_demo.launch

結果を確認する

もし一匹目のturtleを動かしたら、新しいフレームを加えたのにもかかわらず、以前のチュートリアルと動きが変わっていないことに気づくと思います。それは、特別なフレームを加えることは他のフレームになんら影響を与えないからで、listenerは今はまだ以前に定義されたフレームを使用しています。それでは、ここでlistenerの振る舞いを変えてみましょう。

src/turtle_tf_listener.cppを開いて、"/turtle1""/carrot1"に26-27行あたりを置き換えてください:

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

心強いことに、ただリビルドをして再スタートするだけで、2匹目の亀がcarrotを1匹目の亀の代わりに追いかけているのが見えると思います。carrotはturtle1の左2メートルを常に動いていることを覚えていてください。そこには、キャロットに該当する視覚的な表示はそこにありませんが、それを追っかけて2匹目の亀が動いているのが見えると思います。

 $ catkin_make
 $ roslaunch learning_tf start_demo.launch

 $ make
 $ roslaunch learning_tf start_demo.launch

動いているフレームをブロードキャストする

このチュートリアルで配信するようにした余分なフレームは、親のフレームに対して時間がたっても位置が変わらないものとなっています。しかし、もし動いているフレームを配信したければ、時間で変化するようにbroadcasterを変えることができます。

/carrot1のフレームが/turtle1に呼応して、時間が経つにつれて位置が変わるようにframe_tf_broadcaster.cppを修正しましょう。

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

再度turtle demoをリビルドして、再スタートしてください。

 $ catkin_make
 $ roslaunch learning_tf start_demo.launch

 $ make
 $ roslaunch learning_tf start_demo.launch

これで、次のtfと時間についてのチュートリアルに進む準備ができました。(Python) (C++)

Wiki: ja/tf/Tutorials/Adding a frame (C++) (last edited 2014-12-05 13:51:50 by Moirai)