tfでタイムトラベル！(C++)

前回の発展系で、時系列を遡って亀に指令を出してみましょう。

http://www.ros.org/wiki/tf/Tutorials/Time%20travel%20with%20tf%20(C%2B%2B)

前回書き換えた部分(src/turtle_tf_listener.cpp)をさらに以下のようにします。

try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);

見れば分かると思いますが、５秒前のturtle1のデータを使ってturtle2に指令値を与えます。

うーん、ぐちゃぐちゃですね。

5秒前の亀１と亀２の位置を使って現在の亀にフィードバックしちゃっているので、

むちゃくちゃな制御になります。

５秒前の亀１のデータと今の亀２の位置のデータをつかって今の亀にフィードバックすれば、

５秒前の亀１を追いかけるはずですね。

実は以下のように、時間は２つのリンク（座標系）にばらばらに指定ができます。

try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);

"/world"を入れているところには、時間が変わっても動かないリンクを入れる必要があります。

過去の時間でまず/turtle1から/worldへの変換を計算し、現在で/worldと/turtle2の変換を計算します。下図参照。

これで５秒前のところを追いかける亀ができました。

これでtfのチュートリアルは終了です。

座標系（リンク）間の関係を求めるのにtfを使いました。