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.

tfのbroadcasterを書く(C++)

catkin rosbuild

次の２つのチュートリアルで、tf introductionのチュートリアルのデモを再現するコードを書いていきます。その後、続くチュートリアルでは、さらにtfの発展した特長を使ってデモをより拡張していくことに重点を置いていきます。

始める前に、このプロジェクトのために新しいrosパッケージを作る必要があります。ワークスペースの中に、tf,roscpp, rospy , turtlesimに依存したlearning_tfと呼ばれるパッケージを作ります:

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/src
 $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/
 $ catkin_make

 $ roscd tutorials
 $ roscreate-pkg learning_tf tf roscpp rospy turtlesim
 $ rosmake learning_tf

座標系フレームの変化をブロードキャストする方法

このチュートリアルでは、座標系フレームをtfにどのようにブロードキャストするかを学びます。今回の場合、ユーザが亀を動かしたときにturtlesの座標系フレームが変わったことをブロードキャストします。

ソースファイルをまず作ってみましょう。それでは今作ったパッケージに移りましょう。:

 $ roscd learning_tf

コード

src/と呼ばれるフォルダをつくり、エディタを立ち上げ、以下のソースコードをsrc/turtle_tf_broadcaster.cppとして、コピー＆ペーストして保存してください。

melodic noetic   Show EOL distros: 

https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/src/turtle_tf_broadcaster.cpp

   1 #include <ros/ros.h>
   2 #include <tf/transform_broadcaster.h>
   3 #include <turtlesim/Pose.h>
   4 
   5 std::string turtle_name;
   6 
   7 
   8 
   9 void poseCallback(const turtlesim::PoseConstPtr& msg){
  10   static tf::TransformBroadcaster br;
  11   tf::Transform transform;
  12   transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  13   tf::Quaternion q;
  14   q.setRPY(0, 0, msg->theta);
  15   transform.setRotation(q);
  16   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
  17 }
  18 
  19 int main(int argc, char** argv){
  20   ros::init(argc, argv, "my_tf_broadcaster");
  21   if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  22   turtle_name = argv[1];
  23 
  24   ros::NodeHandle node;
  25   ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  26 
  27   ros::spin();
  28   return 0;
  29 };

  transform.setRotation( tf::Quaternion(0, 0, msg->theta) );

  transform.setRotation( tf::Quaternion(0, 0, msg->theta) );

コード解説

では、tfにturtleのポーズを配信している部分に関係しているコードを見ていきましょう。

   2 #include <tf/transform_broadcaster.h>
   3 

tfパッケージは、transforms（座標変換）を簡単に配信できるようにTransformBroadcaster の実行を提供してくれます。TransformBroadcaster を使うには、tf/transform_broadcaster.hをインクルードします。

  10   static tf::TransformBroadcaster br;

ここは後でケーブルを通して、変換を送るのに使うTransformBroadcasterオブジェクトを作っています。

  11   tf::Transform transform;
  12   transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  13   tf::Quaternion q;
  14   q.setRPY(0, 0, msg->theta);

ここでは、Transformオブジェクトを作成し、2Dのturtleのポーズから3d transformに情報をコピーします。

  15   transform.setRotation(q);

ここで回転をセットします。

  16   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

これが、実際に作業を行っている部分です。TransformBroadcasterでtransformを送るのには、４つの引数が必要です。

1. まず、transform自体を渡します。
2. さらに、配信されたtransformとともに、現在の時間をros::Time::now()でtimestampとして渡します。

3. そして、今作っているリンクの親フレームの名前を渡す必要があります。今回の場合は"world"です。

4. 最後に、今作っているリンクの子フレームを渡す必要があります。今回の場合は、turtle自身の名前です。

注意: sendTransform と StampedTransform 親と子の順番が反対ですので気をつけてください。

ブロードキャスターを実行する

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

  rosbuild_add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)

 $ make

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

 $ catkin_make

これでこのデモのためのlaunchファイルを作る段階に入れます。テキストエディタで、新しくstart_demo.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" />

  </launch>

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

 $ roslaunch learning_tf start_demo.launch

一匹の亀だけがいるturtle simが見えているはずです。

結果を確認する

さて、実際にturtleのポーズがtfにブロードキャストできているかをtf_echoを使って確認してみましょう。:

 $ rosrun tf tf_echo /world /turtle1

これは、１匹目の亀のポーズが表示されるはずです。矢印キーを使って亀を動かしてください。（シミュレータではなくターミナルのウィンドウがアクティブになるようにしてください）ここでtf_echoをworldとturtle 2の間のtransformを表示するようにしても、もちろん２匹目の亀はまだ準備していないのでtransformはまだ見られませんが、次のチュートリアルで２匹目の亀を加えたとたん、turtle2はポーズをブロードキャストします。

実際にtfへのtransforms broadcastを使う際には、次のチュートリアルでtf listenerを作らなくてはなりません。(Python) (C++)

フィードバック

もし、このチュートリアルについてコメントがある場合は以下にお願いします。: