ROS講座17 tf-2broadcast、listen

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

ユーザーノードからの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は時間もついていますので時間変化する関係を記述できます。その代わり常に出力し続ける必要があります。

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もあります。

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の依存を追加します。

vis_lecture/CMakeList.txtに追加1

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf        #ここにtf関係を追加
  tf2
  tf2_ros
)

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

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})

vis_lecture/package.xmlに追加

<build_depend>tf</build_depend>
<exec_depend>tf</exec_depend>

launch

/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>

ビルド

cd ~/catkin_ws
catkin_make

実行

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

roslaunch vis_lecture tf_broadcast_listen.launch 

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

rvizの設定とその保存

rvizでうまく表示できていない場合は以下の操作をしてください

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

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

/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" />

tfのプレーム名

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

参考

tfのbroadcast
tfのlisten
Transformer型リファレンス
TransformListener型リファレンス
TransformBroadcaster型リファレンス

目次ページへのリンク

ROS講座の目次へのリンク