tfを使っての座標系の操作と、その発行を行います。
Writing a tf broadcaster (C++)
http://www.ros.org/wiki/tf/Tutorials/Writing%20a%20tf%20broadcaster%20(C%2B%2B)
まず、以下のようにして、新しいlearning_tfというパッケージを作ります。
$ roscd sandbox
$ roscreate-pkg learning_tf tf roscpp rospy turtle_teleop
$ rosmake learning_tf
roscpp, rospy, turtle_teleopに依存するパッケージという意味です。
（私の環境ではroscreate-pkgのos.makedirsで失敗しました。
ros/tools/roscreate/src/roscreate/roscreatepkg.pyの、 L77のos.makedirs(py_path)をコメントアウトして対応しました。）
そして
$ roscd learning_tf
します。
それではいつものように 以下をsrc/turtle_tf_broadcaster.cppとして保存してください。
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
static std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
transform.setRotation(tf::Quaternion(msg->theta, 0, 0));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name + "/pose",
10, &poseCallback);
ros::spin();
return 0;
}
では一行ずつ見ていきます。
#include <tf/transform_broadcaster.h>
transform_broadcasterを使って簡単に座標系の情報を発行（publish)することができるので、
そのためにインクルードしています。
static tf::TransformBroadcaster br;
そしてこれがそのインスタンスです。
tf::Transform transform;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
transform.setRotation(tf::Quaternion(msg->theta, 0, 0));
tf::Transformクラスは位置と姿勢を持つ座標系を表すクラスです。
実はこれはbulletという衝突検知ライブラリのbtTransformです（ここ参照）。
同様にVector3, QuaternionもそれぞれbtVector3、btQuaternionをtypedefしたものです。
亀の２次元座標を３次元のtf::transformに入れています。
チュートリアルではクオータニオンの説明も一切なく、分からない人はもうここで門前払いを喰らいますね。
ROS恐るべし。
やはりロボット系大学院修士卒程度の知識は前提にされていると思います。
なので私も説明しません。
bulletもほとんど使ったことないですが、大体幾何学ライブラリは似たようなものなので、
何か使ったことがあれば大丈夫でしょう。
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
brはTransformBroadcasterですので、これを使って、タイムスタンプ付きの座標系をブロードキャストしてもらいます。
tf::StampedTransformに、座標系(transform)と、タイムスタンプとして使う時刻（ros::Time::now()）、transformの親とする座標系の名前(world)、今作っている座標系の名前(turtle_name)を引数として与えています。
チュートリアルに、
Note: sendTransform and StampedTransform have opposite ordering of parent and child.
とありますが、これは、
|
viod tf::TransformBroadcaster::sendTransform
(const Transform& transform,
const ros::Time& time,
const std::string& frame_id,
const std::string& parent_id)
| | | |
という引数を取るメンバ関数がtf::TransformBroadcasterにあるからです。
最後の引数が親座標系になっていて、その前が自分のIDです。
しかし、この関数はROSCPP_DEPRECATEDがついているので、使わない方がいいでしょう。
その他のコードはお決まりで、ノードを初期化して、亀の/poseというトピックに対し、poseCallbackというコールバック関数をバインドしています。
以上でソースの説明はおしまいです。
ではお決まりの手順でmakeしましょう。
CMakeLists.txtの最後に以下を追加。
rosbuild_add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
そして
$ make
です。
また、以下のようなstart_demo.launchというファイルを作成しておきましょう。
<launch>
<include file="$(find turtle_teleop)/launch/turtle_keyboard.launch" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
</launch>
復習になりますが、これで以下と同じことが実行されますね。
$ roslaunch turtle_teleop turtle_keyboard.launch
$ rosrun learning_tf turtle_tf_broadcaster /turtle1 __name:=turtle1_tf_broadcaster
$ rosrun learning_tf turtle_tf_broadcaster /turtle2 __name:=turtle2_tf_broadcaster
ではそれを意識しつつ、以下を実行しましょう。
$ roslaunch learning_tf start_demo.launch
亀がでましたか？
$ rosrun tf tf_echo /world /turtle1
すると、亀の座標（/worldに対する/turtle1の座標）が表示されたと思います。
亀をキーボードで操作して値が変わることを確認してください。
