ROS講座17 tf-2broadcast、listen

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ユーザーノードからのtf扱い方を解説します。直接`/tf`のROSトピックを扱ってもよいですが、便利なapiがあるのでそれを使うのが一般的です。通常のROSトピックのpub、subとは作法が違いそれぞれbroadcastとlistenと呼ばれます。

## tfのbroadcast
ROSトピック`/tf`に対して

* parent(座標関係の親)のフレームid
* child(座標関係の子)のフレームid
* pose(座標関係を表す位置と姿勢)
* この関係が存在した時刻

を送信します。

またtfのbroadcastには2種類があり、staticなものとdynamicなものがあります。staticなものは一度broadcastすれば永続的に有効なもので固定の関係（例：機体とカメラ位置）を表すのに使い、`/tf_static`でpub-subされます。dynamicなものは位置と一緒に時間も使うもので刻々と変わる位置関係（例：地図の基準と機体）を表すのに使い、`/tf`でpub-subされます。

# tfのlisten
ROSトピック`/tf`と`/tf_static`をsubscriveして過去の一定時間分（標準だと10秒）を内部で保持します。cppのapiで

* parent(座標関係の親)のフレームid
* child(座標関係の子)のフレームid
* 時刻

をリクエストされたときに、すでに受信しているデータを使って解決して

* pose(座標関係を表す位置と姿勢)

を返します。

# tfとtf2
tfを扱うライブラリとしてはtfとtf2のつがあります。どちらのライブラリもROSに入っていますが、ほかのライブラリも徐々にtfからtf2に移っているので、tf2を使うようにしましょう

# ソースコード

## tfのbroadcast
* `broadcast_static_tf()`はstatic_tfの出力をまとめた関数です。
    * `/tf_static`をbroadcastするクラスが`static tf2_ros::StaticTransformBroadcaster`です。pub-subをするので、永続する必要があります。グローバル領域で宣言するか、staticにしましょう。
    * `geometry_msgs::TransformStamped`型で親子関係を記述して`sendTransform()`で出力します。
* `broadcast_dynamic_tf`はdynamic_tfのの出力をまとめた関数です。
    * `/tf`をbroadastするクラスが`static tf2_ros::TransformBroadcaster`です。pub-subをするので、永続する必要があります。グローバル領域で宣言するか、staticにしましょう。
    * `geometry_msgs::TransformStamped`型で親子関係を記述して`sendTransform()`で出力します。
    * static_ftと違ってdynamic_tfは時間もついていますので時間変化する関係を記述できます。その代わり常に出力し続ける必要があります。

```c++:vis_lecture/src/vis_tf_broadcast.cpp
#include <ros/ros.h>
#include <cstdio>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

class BroadCasterTest
{
public:
  BroadCasterTest() : nh_()
  {
    broadcast_static_tf();
    timer_ = nh_.createTimer(ros::Duration(0.1), [&](const ros::TimerEvent& e) {
      broadcast_dynamic_tf();
      counter_++;
    });
  }

private:
  void broadcast_static_tf(void)
  {
    geometry_msgs::TransformStamped static_transformStamped;
    static_transformStamped.header.stamp = ros::Time::now();
    static_transformStamped.header.frame_id = "base_link";
    static_transformStamped.child_frame_id = "static_tf";
    static_transformStamped.transform.translation.x = 0.0;
    static_transformStamped.transform.translation.y = 0.0;
    static_transformStamped.transform.translation.z = 0.3;
    tf2::Quaternion quat;
    quat.setRPY(0.0, 0.0, 0.0);
    static_transformStamped.transform.rotation.x = quat.x();
    static_transformStamped.transform.rotation.y = quat.y();
    static_transformStamped.transform.rotation.z = quat.z();
    static_transformStamped.transform.rotation.w = quat.w();
    static_br_.sendTransform(static_transformStamped);
  }
  void broadcast_dynamic_tf(void)
  {
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "base_link";
    transformStamped.child_frame_id = "dynamic_tf";
    transformStamped.transform.translation.x = 0.3 * cos(counter_ * 0.1);
    transformStamped.transform.translation.y = 0.3 * sin(counter_ * 0.1);
    transformStamped.transform.translation.z = 0.0;
    tf2::Quaternion q;
    q.setRPY(0, 0, 0.0);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();
    dynamic_br_.sendTransform(transformStamped);
  }
  ros::NodeHandle nh_;
  ros::Timer timer_;
  tf2_ros::TransformBroadcaster dynamic_br_;
  tf2_ros::StaticTransformBroadcaster static_br_;
  int counter_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_static_tf2_broadcaster");
  BroadCasterTest broadcast_test;
  ros::spin();
  return 0;
};
```

## tfのlisten
broadcasterの対になるのがlistenerです。2つのフレーム間のtransformを直接取得するほかに、あるposeをほかのframeから見たposeに変換するapiもあります。

```c++:vis_lecture/src/vis_tf_listen.cpp
#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <turtlesim/Spawn.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

class ListenTest
{
public:
  ListenTest() : nh_(), tfBuffer_(), tfListener_(tfBuffer_)
  {
    timer_ = nh_.createTimer(ros::Duration(0.5), [&](const ros::TimerEvent& e) {
      geometry_msgs::TransformStamped transformStamped;
      try
      {
        transformStamped = tfBuffer_.lookupTransform("base_link", "dynamic_tf", ros::Time(0));
      }
      catch (tf2::TransformException& ex)
      {
        ROS_WARN("%s", ex.what());
        return;
      }
      auto& trans = transformStamped.transform.translation;
      ROS_INFO("world->dynamic_tf: %f %f %f", trans.x, trans.y, trans.z);

      geometry_msgs::Pose object_d, object_w;
      object_d.position.z = 1.0;
      object_d.orientation.w = 1.0;
      tf2::doTransform(object_d, object_w, transformStamped);
      ROS_INFO("object_w x:%f, y:%f, z:%f", object_w.position.x, object_w.position.y, object_w.position.z);
    });
  }

private:
  ros::NodeHandle nh_;
  ros::Timer timer_;
  tf2_ros::Buffer tfBuffer_;
  tf2_ros::TransformListener tfListener_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_tf2_listener");
  ListenTest listen_test;
  ros::spin();
  return 0;
};
```

* "tf2_ros::TransformListener ln"の実体化のタイミング  
これは内部でNodeHandleを使用しているので必ずNodeHandleを宣言した後に実体化してください。例えばグローバル空間でtf::TransformListenerを宣言すると実行時エラーになります。またこのインスタンスは裏で`/tf`と`/static_tf`をsubscribeしています。変換をする直前にインスタンス化をしてもこれらのROSトピックを十分に受信できないので早めにインスタンス化をする必要があります。
* lookupTransformの引数のstamp
このstampで指定した時間におけるtfの関係を使ってtfを解決しようとします。そのために現在( ros::Time::now() )を指定すると**現在の時刻以降のtf**がないと変換できません。つまりエラーになります。ある時間を解決できるこれらのメッセージを受信するまで待つのが`waitForTransform()`です。  
時間をあまり気にせず、その時点での最新のtfを使えば良いという場合はstampに`ros::Time(0)`を指定します。
* 例外処理の対応  
起動したてで該当のtfが出ていない場合などでは`lookupTransform()`は必ず例外を出します。なのでtf::TransformListenerのapiを使うときは必ずtry catchをしてください。

## buildの設定
CmakeList.txtとpackage.xmlにtfの依存を追加します。

```cmake:vis_lecture/CMakeList.txtに追加1
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf        #ここにtf関係を追加
  tf2
  tf2_ros
)
```

実行とライブラリの設定をします。

```cmake:vis_lecture/CMakeList.txtに追加2
add_executable(vis_tf_broadcast src/vis_tf_broadcast.cpp)
add_executable(vis_tf_listen src/vis_tf_listen.cpp)

target_link_libraries(vis_tf_broadcast ${catkin_LIBRARIES})
target_link_libraries(vis_tf_listen ${catkin_LIBRARIES})
```

```xml:vis_lecture/package.xmlに追加
<build_depend>tf</build_depend>
<exec_depend>tf</exec_depend>
```

## launch
```xml:/vis_lecture/launch/tf_broadcast_listen.launch
<launch>
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />
  
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
  <node name="vis_tf_broadcast" pkg="vis_lecture" type="vis_tf_broadcast"/>
  <node name="vis_tf_listen"    pkg="vis_lecture" type="vis_tf_listen" output="screen"/>
</launch>
```

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell
roslaunch vis_lecture tf_broadcast_listen.launch 
```

![vis_tf.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/1e45b656-059d-d85c-7fd7-18a8d00512f5.gif)


コンソールにはbase_link->body_linkのtransformの値が表示され、Rviz画面上ではtfがVisualizeされています。

## rvizの設定とその保存
rvizでうまく表示できていない場合は以下の操作をしてください

* FixedFrameをbase_linkにする。
* 左下のaddからtfを選択する。

![vis_tf_rviz.png](https://qiita-image-store.s3.amazonaws.com/0/254442/130b5d01-04be-967d-ae09-ef01bf0199b5.png)

これらの画面設定はrvizのメニューバーの「Save Config As」でコンフィグファイルを保存できます。またRvizのGUIからコンフィグをロードすることもできますが、以下のようにlaunchを書くことでも読み込めます。

```xml:/vis_lecture/launch/tf_broadcast_listen.launchの一部
<arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
```

![vis_tf.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/19e9fd8a-fcc8-a120-c65a-7b99bc4f0ce7.gif)


# tfのプレーム名
tfのフレーム名には`group/framename`のようにスラッシュを使うことができます。しかし`/group/frame_name`のように頭にスラッシュを付けることは推奨されなくエラーが出る場合があります。
ノードによって頭の`/`を無視するものがあったりなかったりするのでつけないことを強く推奨します。

#参考
[tfのbroadcast](http://wiki.ros.org/ja/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29)
[tfのlisten](http://wiki.ros.org/ja/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2B%29)
[Transformer型リファレンス](http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transformer.html)
[TransformListener型リファレンス](http://docs.ros.org/diamondback/api/tf/html/c++/classtf_1_1TransformListener.html)
[TransformBroadcaster型リファレンス](http://docs.ros.org/lunar/api/tf/html/c++/classtf_1_1TransformBroadcaster.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)