ROSのTFとは？簡単に説明（サンプルコード付き）②

前回、2つの座標系間の位置関係が変化しない場合のTFの発信をしました。今回は常に位置関係が変化する時のTFについて説明します。

動的TF

ロボットを扱っていると、常に位置関係が変化する2つの座標系を表したい時がよくあります。例えば部屋から見たロボットの位置だったり、アームロボットの手先の位置を表すのにもTFを使います。今回は変化するTFをどう発信するか説明します。

といっても基本的な考え方は静的TFの時と同じです。まずはサンプルプログラムを載せます。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main (int argc, char** argv){
  ros::init(argc, argv, "tf_publisher");
  ros::NodeHandle nh;
  ros::Rate loop_rate(10);

  tf::TransformBroadcaster odom_broadcaster;

  double t = 0;

  while(ros::ok()){
    ros::spinOnce();

    geometry_msgs::TransformStamped odom_trans;

    //現在時刻、親フレーム、子フレームの指定
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = "map";
    odom_trans.child_frame_id = "base_link";

    //親フレームからみた子フレームの位置
    odom_trans.transform.translation.x = t;
    odom_trans.transform.translation.y = sin(3.14*t);
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(cos(3.14*t));

    //TFを一回発信
    odom_broadcaster.sendTransform(odom_trans);

    t += 0.01;

    loop_rate.sleep();
  }
}

順番に見ていくと、まず最初にtransform_broadcasterをインクルードします。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

続いてmain関数の最初の4行目でgeometry_msgs::TransformStamped型の変数を作っています。そんでそのヘッダ部分に現在時刻と親フレーム、子フレームの情報を入れます。

geometry_msgs::TransformStamped odom_trans;

//現在時刻、親フレーム、子フレームの指定
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "map";
odom_trans.child_frame_id = "base_link";

そのあとが今回の本題、TFの座標を入れる部分です。x,y,zと回転を入れます。回転はクオタニオン(四元数)で表現します。クオタニオン表現では3次元ベクトルとそのベクトル周りの回転量で姿勢を表しますが、2次元平面だけで考える場合はヨー角からクオタニオンを作ってくれる便利な関数があるので非常に楽です。

//親フレームからみた子フレームの位置
odom_trans.transform.translation.x = t;
odom_trans.transform.translation.y = sin(3.14*t);
odom_trans.transform.translation.z = 0.0;

//ヨー角からクオタニオンを作ってrotationに入れる
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(cos(3.14*t));

TFの準備ができたところで次の関数で発信します。

//TFを一回発信
odom_broadcaster.sendTransform(odom_trans);

これを一定時間ごとに繰り返すだけで、動的TFの発信は終わりです。
今回はsin曲線を描きつつ進行方向（＝微分方向＝cos）を向くようにしたのでこんな感じの軌跡ができるはずです。

ROSを最初から勉強したい方はこちら