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の問題をデバッグする

melodic noetic   Show EOL distros: 

このチュートリアルは典型的なtfの問題をデバッグする手順を一通り解説しています. tfのトラブルシューティング・ガイドで説明されている手順をturtlesimを使った例に適用していきます. また, たくさんのtf debugging toolsも使用します.

例題の開始

このチュートリアルにあたり, いくつかの問題を含むデモアプリをセットアップします. このチュートリアルのゴールは, これらの問題を特定する体系的アプローチを適用することです.

  $ roscd turtle_tf
  $ rosmake

  $ catkin_make 
  $ roscd turtle_tf

そして, 実行してから何が起こるかを確認してみましょう:

  $ roslaunch start_debug_demo.launch

turtlesimが立ち上がるのが確認できます. デモを立ち上げたターミナルのウインドウを選択を選択すると, 矢印キーを使ってロボットのうちの一体を動き回らせることができます. 左上に二番目のロボットがいます.

デモが正常に機能しているのであれば, この二番目のロボットが矢印キーで操作しているロボットを追いかけているはずです. もちろん, そうはならず... なぜなら, 最初にいくつかの問題を解決しなければならないからです. 考えるべきは, 以下のエラーメッセージです:

[ERROR] [1412926632.510259133]: "turtle3" passed to lookupTransform argument target_frame does not exist. 

tfのリクエストを見つける

つまり, debugging tf problems guideに従った時, まずtfに,正確には何をさせたいのかを明確にする必要があります. 従って, 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::Stamped<tf::Transform> transform;
  25     try{
  26       listener.lookupTransform("/turtle3", "/turtle1",
  27                                ros::Time::now(), 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 * 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 };

   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("/turtle3", "/turtle1",
  27                                ros::Time::now(), transform);
  28     }
  29     catch (tf::TransformException &ex) {
  30       ROS_ERROR("%s",ex.what());
  31       ros::Duration(1.0).sleep();
  32     }
  33 
  34     geometry_msgs::Twist vel_msg;
  35     vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
  36                                     transform.getOrigin().x());
  37     vel_msg.linear.x = 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 };

   1     try{
   2       listener.lookupTransform("/turtle3", "/turtle1",
   3                                ros::Time::now(), transform);
   4     }

では, このtfに対するリクエストがなぜ失敗しているかを見ていきましょう.

フレームをチェックする

まず、tfで/turtle3 と /turtle1のtransformが出来ているかを明らかにしたほうがよさそうです：

  $ rosrun tf tf_echo turtle3 turtle1

フレームturtle3は存在しないと出力しています。

Exception thrown:Frame id /turtle3 does not exist! When trying to transform between /turtle1 and /turtle3.
The current list of frames is:
Frame /turtle1 exists with parent /world.
Frame /world exists with parent NO_PARENT.
Frame /turtle2 exists with parent /world.

Failure at 1412928031.552827236
Exception thrown:"turtle3" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:
Frame turtle1 exists with parent world.
Frame turtle2 exists with parent world.

上記のメッセージの最後の3行は、どのフレームがすでに形成されているかを明示しています。これをグラフィカルな表示で確認したければ、以下を実行します：

  $ rosrun tf view_frames
  $ evince frames.pdf

そして、以下のアウトプットが得られます。

これで、明らかに、存在していないturtle3に対してリクエストしていることが問題です。このバグを修正するために、25-28行目のturtle3をturtle2に置き換えます：

   1   try{
   2     listener.lookupTransform("/turtle2", "/turtle1",
   3                              ros::Time::now(), transform);
   4   }

そして、一度実行中のデモ(Ctrl-c)を止めて、ビルドし、再びデモを実行します：

  $ make
  $ roslaunch start_debug_demo.launch

  $ catkin_make
  $ roslaunch start_debug_demo.launch

すると、すぐに次の問題に遭遇します：

[ERROR] 1254264620.183142000: You requested a transform that is 0.116 miliseconds in the past,
but the most recent transform in the tf buffer is 3.565 miliseconds old.
 When trying to transform between /turtle1 and /turtle2.

[ERROR] [1412928431.849367634]: Lookup would require extrapolation into the future.  Requested time 1412928431.849080666 but the latest data is at time 1412928431.841423988, when looking up transform from frame [turtle1] to frame [turtle2]

タイムスタンプをチェックする

先ほど、フレームネーム問題を解決したので、次はタイムスタンプを見ていきます。"now()"時の turtle2 と turtle1の間の変化をtransformしようとしていることを思い出して下さい。タイミングに関する統計を取るために、以下を実行します：

  $ rosrun tf tf_monitor turtle2 turtle1

結果はこのようなものになるはずです：

RESULTS: for turtle2 to turtle1
Chain is: world -> world
Net delay     avg = 0.00810157: max = 0.0170012

Frames:

All Broadcasters:
Node: /turtle1_tf_broadcaster 62.8465 Hz, Average Delay: 0.000513553 Max Delay: 0.00231993
Node: /turtle2_tf_broadcaster 62.8465 Hz, Average Delay: 0.000546285 Max Delay: 0.00111162

ここの重要な箇所はturtle2 から turtle1へのチェーンにかかる遅延です。アウトプットは、8ミリ秒の平均遅延時間があるのを示しています。これは、8ミリ秒経過後にだけ、tfが２つのturtle間のtransformができることを意味しています。従って、"now()"ではなく、8ミリ秒前のturtle間のtransformをtfに要求しておけば、tfは、時々応答することができるでしょう。25-28行目を次のように変更したものでテストしてみましょう：

   1   try{
   2     listener.lookupTransform("/turtle2", "/turtle1",
   3                              ros::Time::now()-ros::Duration(0.1), transform);
   4   }

この新しいコードの中では、100ミリ秒前のturtle間のtransformを要求しています(なぜ8でないか？単に安全のためです...）。デモ(Ctrl-c)を止めて、ビルドし、実行します：

  $ make
  $ roslaunch start_debug_demo.launch

  $ catkin_make
  $ roslaunch start_debug_demo.launch

そして、ついにturtleが動くのを見られるはずです！

先ほどの最後の修正は、実際にしたいことではなく、問題だったタイムスタンプについて単に確かめるものになっていました。本当の修正はこのようになるでしょう：

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

またはこのように：

   1   try{
   2     ros::Time now = ros::Time::now();
   3     listener.waitForTransform("/turtle2", "/turtle1", 
   4                               now, ros::Duration(1.0));
   5     listener.lookupTransform("/turtle2", "/turtle1",
   6                              now, transform);
   7   }