覚え書きブログ

ROSの覚え書き(tf broadcaster)

gmappingを利用するために、ロボット座標フレームをtfでブロードキャストするように、使用しているロボットのプログラム変更してみた。

tf broadecasterで参考にしたのは、下記のページ:
http://wiki.ros.org/ja/tf/Tutorials/Writing a tf broadcaster (C++)
http://wiki.ros.org/ja/navigation/Tutorials/RobotSetup/Odom#tf.2BMJJ1KDBEMF8wqjDJMOEwyDDqMG5ZCWPbMG6RTU.2Fh-

以下のように、元々nav_msgs/Odometryをpublishしていたところに、TFMessage型のodom_transのx,y,z座標およびQuotanionを設定し、sendTransformでブロードキャストしている。また、frame_idとchild_frame_idとして"odom"と"base_link"を設定している。

//===================
//added by Hachiya, 2017/07/21
#include <tf/transform_broadcaster.h>
geometry_msgs::TransformStamped odom_trans;

odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";

odom_trans.header.stamp = m_odom.header.stamp;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = odom.pose.pose.position.x;
odom_trans.transform.translation.y = odom.pose.pose.position.y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom.pose.pose.orientation;

//send the transform
tf::TransformBroadcaster odom_broadcaster;
odom_broadcaster.sendTransform(odom_trans);
//===================

以下のようにクライアント側では、トピックに/tfが見えるようになっているのがわかる。また、transformメッセージをechoできるようになっている。

> rostopic list -v
...
* /tf [tf2_msgs/TFMessage] 1 publisher
...

> rostopic echo /tf
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1491075157
        nsecs: 870166670
      frame_id: odom
    child_frame_id: base_link
    transform: 
      translation: 
        x: 0.275995326421
        y: 0.156077342982
        z: 0.0
      rotation: 
        x: 0.00344508371221
        y: -0.00242076886391
        z: 0.355463455241
        w: 0.934680695887

さらに、ロボット座標(odomとbase_linkの関係)は以下のように取得できるのがわかる。

> rosrun tf view_frames
Listening to /tf for 5.000000 seconds
Done Listening
dot - graphviz version 2.36.0 (20140111.2315)

Detected dot version 2.36
frames.pdf generated
>evince frames.pdf

f:id:hirotaka_hachiya:20170721184819p:plain