| Note: This tutorial assumes that you have completed the Transform Configuration tutorial. All the code for this tutorial is available in the odometry_publisher_tutorial package.. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Publishing Odometry Information over ROS
Description: This tutorial provides an example of publishing odometry information for the navigation stack. It covers both publishing the nav_msgs/Odometry message over ROS, and a transform from a "odom" coordinate frame to a "base_link" coordinate frame over tf.Tutorial Level: BEGINNER
ROS上でのオドメトリ情報の配信
ナビゲーションスタックは環境中でのロボットの位置を特定し、センサデータを地図に反映するために tf を用います。ただ、tfはロボットの速度に関する情報はなにも与えません。そのため、ナビゲーションスタックは変換情報、及び速度情報を持つROS上のnav_msgs/Odometryメッセージとの両方を配信するオドメトリの情報源を必要とします。このチュートリアルは、nav_msgs/Odometryメッセージを説明し、ROSのnav_msgs/Odometryメッセージとtfの変換情報を配信するサンプルコードを示します。
nav_msgs/Odometry メッセージ
nav_msgs/Odometry メッセージはフリースペースにおけるロボットの位置と速度の推定値を記録したものです。
# This represents an estimate of a position and velocity in free space. # The pose in this message should be specified in the coordinate frame given by header.frame_id. # The twist in this message should be specified in the coordinate frame given by the child_frame_id Header header string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/TwistWithCovariance twist
このメッセージのpose（姿勢）はodometric frameにおけるロボットの推定位置に対応し、その推定値の確かさを表す共分散も記録することができます。また、このメッセージにおけるtwistはchild frameにおけるロボットの速度と、速度の推定値の共分散に対応します。child frameは通常、mobile baseの座標系となります。
tfを用いたオドメトリの変換の配信
Transform Configurationチュートリアルで議論したように、 "tf" ソフトウェアライブラリは、ロボットに関する座標系同士の関係をtransformツリーで管理します。任意のオドメトリの情報源は、オドメトリを計測している座標系に基づいて情報を配信しなければなりません。下のコードはtfの基本的な知識を前提としたもので、Transform Configurationの内容を読めば十分理解できるものです。
コードを書く
ここでは 円弧上を動くだけの仮想のロボットに対し、ROS上への nav_msgs/Odometry メッセージ及び tfを使った変換情報を配信するためのサンプルコードを書きます。コード全体を先に示し、後で各部分について説明します。
あなたのパッケージのmanifest.xml（TODO: 古い）には次の依存関係を加えておきましょう。
<depend package="tf"/> <depend package="nav_msgs"/>
1 #include <ros/ros.h>
2 #include <tf/transform_broadcaster.h>
3 #include <nav_msgs/Odometry.h>
4
5 int main(int argc, char** argv){
6 ros::init(argc, argv, "odometry_publisher");
7
8 ros::NodeHandle n;
9 ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
10 tf::TransformBroadcaster odom_broadcaster;
11
12 double x = 0.0;
13 double y = 0.0;
14 double th = 0.0;
15
16 double vx = 0.1;
17 double vy = -0.1;
18 double vth = 0.1;
19
20 ros::Time current_time, last_time;
21 current_time = ros::Time::now();
22 last_time = ros::Time::now();
23
24 ros::Rate r(1.0);
25 while(n.ok()){
26
27 ros::spinOnce(); // check for incoming messages
28 current_time = ros::Time::now();
29
30 //compute odometry in a typical way given the velocities of the robot
31 double dt = (current_time - last_time).toSec();
32 double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
33 double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
34 double delta_th = vth * dt;
35
36 x += delta_x;
37 y += delta_y;
38 th += delta_th;
39
40 //since all odometry is 6DOF we'll need a quaternion created from yaw
41 geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
42
43 //first, we'll publish the transform over tf
44 geometry_msgs::TransformStamped odom_trans;
45 odom_trans.header.stamp = current_time;
46 odom_trans.header.frame_id = "odom";
47 odom_trans.child_frame_id = "base_link";
48
49 odom_trans.transform.translation.x = x;
50 odom_trans.transform.translation.y = y;
51 odom_trans.transform.translation.z = 0.0;
52 odom_trans.transform.rotation = odom_quat;
53
54 //send the transform
55 odom_broadcaster.sendTransform(odom_trans);
56
57 //next, we'll publish the odometry message over ROS
58 nav_msgs::Odometry odom;
59 odom.header.stamp = current_time;
60 odom.header.frame_id = "odom";
61
62 //set the position
63 odom.pose.pose.position.x = x;
64 odom.pose.pose.position.y = y;
65 odom.pose.pose.position.z = 0.0;
66 odom.pose.pose.orientation = odom_quat;
67
68 //set the velocity
69 odom.child_frame_id = "base_link";
70 odom.twist.twist.linear.x = vx;
71 odom.twist.twist.linear.y = vy;
72 odom.twist.twist.angular.z = vth;
73
74 //publish the message
75 odom_pub.publish(odom);
76
77 last_time = current_time;
78 r.sleep();
79 }
80 }
全部コードを見ましたね。では以下で重要な部分について細かく見ていきましょう。
"odom"座標系から"base_link"座標系への変換情報と nav_msgs/Odometry メッセージの両方を配信するために、それぞれに対応するヘッダファイルをインクルードします。
ROS、tfそれぞれにメッセージを送るために、ROSのパブリッシャーとtfの tf::TransformBroadcaster を両方作る必要があります。
"odom" 座標系の原点からロボットがスタートすると仮定しています。
"odom" 座標系でx方向に0.1m/s、y方向に-0.1m/s、th（シータ）方向に0.1rad/sという速度をセットします。"base_link" 座標系がこれで動きます。ロボットはこれで円弧を描きます。
24 ros::Rate r(1.0);
簡単にするために1Hzでオドメトリ情報を配信することにします。ほとんどのシステムでは、おそらくこれより早い周期で配信することが望ましいでしょう。
30 //compute odometry in a typical way given the velocities of the robot
31 double dt = (current_time - last_time).toSec();
32 double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
33 double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
34 double delta_th = vth * dt;
35
36 x += delta_x;
37 y += delta_y;
38 th += delta_th;
オドメトリ情報を、先ほど定めた速度に基づいて更新します。この例では速度は定数ですが、実際のシステムではもちろん変動するのでそれに合わせて計算します。
我々は一般に、すべてのメッセージは3Dのものを使うことになります。2Dと3Dの座標を共存して機能させるため、生成されるメッセージを最小限にとどめるためです。ここではそのためにオドメトリのヨーの値をクォータニオンに変換する必要があります。幸い、tfがヨーの値からクォータニオンを生成する関数やクォータニオンからヨーの値を取得する関数を提供しています。
ここで、tfに送る TransformedStamped メッセージを作ります。 current_time （現在時刻）における "odom" 座標系から "base_link" 座標系への変換を配信したいので、メッセージのヘッダと、child_frame_idを適宜セットします。"odom" が親の座標系で "base_link" が子供の座標系になることに注意しましょう。
オドメトリデータから座標変換のデータを代入していき、 TransformBroadcaster を使って送信します。
さらに、ナビゲーションスタックが速度情報を得るために、 nav_msgs/Odometry メッセージを配信する必要があります。こちらも現在時刻をヘッダにセットし、 "odom" 座標系を frame_id にセットします。
62 //set the position
63 odom.pose.pose.position.x = x;
64 odom.pose.pose.position.y = y;
65 odom.pose.pose.position.z = 0.0;
66 odom.pose.pose.orientation = odom_quat;
67
68 //set the velocity
69 odom.child_frame_id = "base_link";
70 odom.twist.twist.linear.x = vx;
71 odom.twist.twist.linear.y = vy;
72 odom.twist.twist.angular.z = vth;
オドメトリの諸情報を配信するデータに代入していきます。速度情報は "base_link" 座標系に対して送るので、child_frame_idは "base_link" に設定します。
