cvl-robot's diary

研究ノート　メモメモ https://github.com/dotchang/

openFrameworksをROSのフロントエンドにする(その2)

この記事では、Intel Euclidの６DOFシナリオの出力をopenFrameworksで受け取る方法を調べていきます。
ROSメッセージの受け取り方は、紹介記事とチュートリアルでなんとなく理解できました。
しかし、6DOFの正体がいまいちわかりませんので、まずこれから調べていきます。

6DOFの出力topicの正体を探る

intel Euclid 6DOF topicなどのキーワードで、ダイレクトにgoogle検索すると、１番最初に
communities.intel.com
なるスレッドが出てきて、３番めの書き込みに/realsense/odomがそれだ、との答えが書いてありました。
そういえば、monitorのところに直接書いてありましたね。
f:id:cvl-robot:20180124122118p:plain

念の為、その出力をrostopic echoで見てみると、

> rostopic echo /realsense/odom
header:
seq: 10397
stamp:
secs: 1516768367
nsecs: 298564143
frame_id: /odom
child_frame_id: camera_link
pose:
pose:
position:
x: 0.010704016313
y: 0.0108824670315
z: -0.0151881296188
orientation:
x: 0.0168443697693
y: -0.0229394350666
z: 0.0387872524864
w: 0.998842157676
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
twist:
linear:
x: -0.00104899965742
y: -0.000815259476818
z: 0.000571808433577
angular:
x: -0.00110118133651
y: 0.000919767173822
z: -0.00199628032458
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    • -

よくわからないパラメータがあるものも、おそらくこれがSLAMの出力結果で間違いなさそうです。[1]によるとTwistは速度・角速度を表しているそうです。

/realsense/odomの正体を探る

topicの名前と中身は確認できましたが、これを受け取るためにはmsgの型を知る必要があります。更に、それをプログラムで受け取るために、型の定義が書かれた.hのヘッダーファイルを入手するか作るかしなければなりません。
rostopic infoで詳細を調べると、

> rostopic info /realsense/odom
Type: nav_msgs/Odometry

Publishers:
* /manager (http://10.42.0.1:36052/)

Subscribers: None

実態は、nav_msgs/Odometryであるとわかります。

Odometrty.hを探す

通常、msgの型定義から、cmakeを通してc++用のヘッダーファイルが作成され、それをincludeすることでROSのプログラムはROSmsgを扱えるようになります。Euclid 6DOFの場合、すでに動いているのでどこかにこのファイルが存在しているはずです。

> locate Odometry.h
/opt/ros/kinetic/include/nav_msgs/Odometry.h

locateで素直に調べたら、一発で出てきました。これをコピーしてofApp.hと同じディレクトリに持ってきます。（パスを通してももちろん良いです。）

Odometry.msgの中を見る

拡張子が.msgのファイルにROSmsgの定義は書かれているはずなので、Odometry.msgというファイルをlocateで探します。

> locate Odometry.msg
/opt/ros/kinetic/share/nav_msgs/msg/Odometry.msg

この中身をみると、

# 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

4つの要素を持っていることがわかります。string以外は聞き慣れない型ですので、また調べる必要がありそうです。
同じように辿って行くと中身が見えてきます。

> locate Header.msg
/opt/ros/kinetic/share/std_msgs/msg/Header.msg

> less /opt/ros/kinetic/share/std_msgs/msg/Header.msg
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id

ただ、面倒くさいので、google先生に聞いてみると、よくまとまった公式ページの情報を教えてくれました。こちらを参照していきます。
nav_msgs/Odometry Documentation

nav_msgs/Odometryの受信用callback関数

以上の情報を整理して、受信部分だけ先に書いてみます。多分こんな感じ。

void ofApp::my_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
        ROS_INFO("header: seq[%d] stamp sec[%d] nsec[%d] frame_id[%s]", msg->header.seq,
                                  msg->header.stamp.sec,
                                  msg->header.stamp.nsec,
                                  msg->header.frame_id.c_str());
        ROS_INFO("child_frame_id: [%s]", msg->child_frame_id.c_str());
        ROS_INFO("Pose: position: [%f][%f][%f]", msg->pose.pose.position.x,
                                                 msg->pose.pose.position.y,
                                                 msg->pose.pose.position.z); 
        ROS_INFO("   orientation: [%f][%f][%f][%f]", msg->pose.pose.orientation.x,
                                                     msg->pose.pose.orientation.y,
                                                     msg->pose.pose.orientation.z,
                                                     msg->pose.pose.orientation.w);
        ROS_INFO("    covariance: [%f]", msg->pose.covariance);

        ROS_INFO("Twist:  linear: [%f][%f][%f]", msg->twist.twist.linear.x,
                                                 msg->twist.twist.linear.y,
                                                 msg->twist.twist.linear.z);
        ROS_INFO("       angular: [%f][%f][%f]", msg->twist.twist.angular.x,
                                                 msg->twist.twist.angular.y,
                                                 msg->twist.twist.angular.z);
        ROS_INFO("    covariance: [%f]", msg->twist.covariance);
}

[1] twist
dailyrobottechnology.blogspot.jp