| Note: This tutorial assumes you have completed the writing a tf broadcaster 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. |
tf listenerを書く (C++)
Description: このチュートリアルでは、どのようにtfを使って座標系フレームの変化を取得するのかを学びますTutorial Level: BEGINNER
Next Tutorial: Adding a frame (C++)
Contents
前回のチュートリアルでは、tf broadcasterをtfにturtleのポーズを配信するために作りました。このチュートリアルでは、tfを使い始めるためにtf listenerを作っていきます。
どのようにtf listenerをつくるか
まず、ソースコードを作りましょう。まず、前回のチュートリアルで作ったパッケージに移動します:
$ roscd learning_tf
コード
エディタを立ち上げ、以下のソースコードをsrc/turtle_tf_listener.cppとして、コピー＆ペーストして保存してください。
https://raw.github.com/ros/geometry_tutorials/groovy-devel/turtle_tf/src/turtle_tf_listener.cpp
1 #include <ros/ros.h>
2 #include <tf/transform_listener.h>
3 #include <turtlesim/Velocity.h>
4 #include <turtlesim/Spawn.h>
5
6 int main(int argc, char** argv){
7 ros::init(argc, argv, "my_tf_listener");
8
9 ros::NodeHandle node;
10
11 ros::service::waitForService("spawn");
12 ros::ServiceClient add_turtle =
13 node.serviceClient<turtlesim::Spawn>("spawn");
14 turtlesim::Spawn srv;
15 add_turtle.call(srv);
16
17 ros::Publisher turtle_vel =
18 node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
19
20 tf::TransformListener listener;
21
22 ros::Rate rate(10.0);
23 while (node.ok()){
24 tf::StampedTransform transform;
25 try{
26 listener.lookupTransform("/turtle2", "/turtle1",
27 ros::Time(0), transform);
28 }
29 catch (tf::TransformException ex){
30 ROS_ERROR("%s",ex.what());
31 ros::Duration(1.0).sleep();
32 }
33
34 turtlesim::Velocity vel_msg;
35 vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
36 transform.getOrigin().x());
37 vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
38 pow(transform.getOrigin().y(), 2));
39 turtle_vel.publish(vel_msg);
40
41 rate.sleep();
42 }
43 return 0;
44 };
Hydroでは、turtlesim/Velocity.hがgeometry_msgs/Twist.hに置き換わり、上記コードではコンパイルが通らないため、以下のコードをsrc/turtle_tf_listener.cppとして保存してください。
https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/src/turtle_tf_listener.cpp
1 #include <ros/ros.h>
2 #include <tf/transform_listener.h>
3 #include <geometry_msgs/Twist.h>
4 #include <turtlesim/Spawn.h>
5
6 int main(int argc, char** argv){
7 ros::init(argc, argv, "my_tf_listener");
8
9 ros::NodeHandle node;
10
11 ros::service::waitForService("spawn");
12 ros::ServiceClient add_turtle =
13 node.serviceClient<turtlesim::Spawn>("spawn");
14 turtlesim::Spawn srv;
15 add_turtle.call(srv);
16
17 ros::Publisher turtle_vel =
18 node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
19
20 tf::TransformListener listener;
21
22 ros::Rate rate(10.0);
23 while (node.ok()){
24 tf::StampedTransform transform;
25 try{
26 listener.lookupTransform("/turtle2", "/turtle1",
27 ros::Time(0), transform);
28 }
29 catch (tf::TransformException &ex) {
30 ROS_ERROR("%s",ex.what());
31 ros::Duration(1.0).sleep();
32 continue;
33 }
34
35 geometry_msgs::Twist vel_msg;
36 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
37 transform.getOrigin().x());
38 vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
39 pow(transform.getOrigin().y(), 2));
40 turtle_vel.publish(vel_msg);
41
42 rate.sleep();
43 }
44 return 0;
45 };
実行している最中に"Lookup would require extrapolation into the past"というエラーを取得したら、次の替えのコードでlistenerを呼び出してみてください。:
try {
listener.waitForTransform(destination_frame, original_frame, ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);
} catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
コード解説
さて、tfにturtleのポーズを配信している部分に関係しているコードを見てみましょう。
tfパッケージは、transforms（座標変換）を簡単に受け取れるようにTransformListener が実装されています。TransformListener を使うには、tf/transform_listener.hをインクルードします。
Error: No code_block found ここで、TransformListenerのオブジェクトを作っています。一旦listenerが作成されると、ケーブルを通してtfのtransformationsを受け取り始め、１０秒間それらをバッファに溜めます。TransformListenerオブジェクトはしっかりと保持されるようにスコープを決めなければ、キャッシュをためることができず、ほぼ全てのクエリが失敗します。一般的な方法は、TransformListenerオブジェクトをクラスのメンバ変数にしてしまうことです。
20 tf::TransformListener listener;
ここで、実際の作業を行っています。特定のtransformationについてlistenerに尋ねます。 ４つの引数を見てみましょう:
- まず、取得したいtransform(変換)のfromにあたるフレームを指定します。
- 取得したいtransform(変換)のtoにあたるフレームを指定します。
transformがしたい時間を指定します。ros::Time(0)を指定すると最新のtransformを取得できます。
- 結果を取得したいオブジェクトを指定します。
これが例外を取得するためにtry-catchに書かれているすべてです。
ここでは、transformは、turtle1からの距離や角度に基づいて、turtle2の直線速度と回転速度を計算するのに使われます。"turtle2/command_velocity"のtopicに新しい速度が配信され、シミュレータはこれによりturtle2の動きを更新します。
listenerを実行する
コードは完成したので、コンパイルしてみましょう。CMakeLists.txtを開いて、以下の行をファイルの最後に追加してください。:
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})パッケージをビルドします;(catkinワークスペースのトップディレクトリにて):
$ catkin_make
ビルドが全て成功すると、devel/lib/learning_tfフォルダの中にturtle_tf_listenerと呼ばれるバイナリファイルが出来ているはずです。
rosbuild_add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
Build your package:
$ make
全てがうまくいったなら、binフォルダの中にturtle_tf_listnerと呼ばれるバイナリファイルがあると思います。
バイナリファイルが生成されていたら、このデモのlaunchファイルを編集しましょう。テキストエディタで、start_demo.launchと呼ばれるファイルを開き、<launch>タグのブロックの中にnodeブロックをマージします。:
<launch>
...
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>まず、前回のチュートリアルのlaunchファイルは止めていることを確認してください(ctrl-cを使ってください)今、完全なturtleのデモを始める準備ができました。:
$ roslaunch learning_tf start_demo.launch
2匹の亀が動いているturtle simが見えていますね。
結果を確認する
tfのbroadcasterがうまく動作しているかを確かめるには、矢印キーを用いて１匹目の亀を操作してみればよいだけです。（シミュレータではなくターミナルのウィンドウがアクティブになるようにしてください）すると、２匹目の亀が１匹目の亀についてくるのを確認できます。
turtlesimが立ち上がったときに以下のようなものが表示されているかもしれません:
[ERROR] [1412840409.742716292]: "turtle2" passed to lookupTransform argument target_frame does not exist.
これは、turtlesimを立ち上げ、tfのフレームをブロードキャストし始めるのに少し時間がかかっていることで生じていて、この時間はlistenerがturtle2のメッセージが届く前に、transform(変換)を計算しようとしているからです。
さて、これでさらに次のチュートリアルへ進むことができます。次は、どのようにフレームを追加するかを学習します。(Python) (C++)
