ROS Kineticで、ARマーカーの姿勢データの取得

動機

ROSのパッケージにある、ar_track_alvarの準備ができたので、つぎは実際にトピックを拾って、マーカーの姿勢をとれるかやってみる。

環境

• Ubuntu 16.04 LTS
• ROS Kinetic Kame
• 今回使用したのUSBカメラ（Logicool C270　UVC対応）1台

参考にした情報

ROSではじめるロボットプログラミング

ARマーカーの姿勢情報の取得

ar_pose_markerのデータ取得

ar_pose_markerトピックをsubscribeするノードを作成する。

特に書いてなかったがこれまで「camera_eval」と名前をつけたパッケージで、カメラ接続の検証などをしていたため、そのパッケージ内のscriptsに、以下のようにコードをつくる。
シンプルに、subscriberとcallbackだけ。

ar_pose_subscriber.py

import rospy
from ar_track_alvar_msgs.msg import AlvarMarkers

class ARPoseSubscriber(object):
    def __init__(self):
        self.sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self.callback_alvmarker)

    def callback_alvmarker(self, markers):
        
        for m in markers.markers:
            marker_id = m.id
            if marker_id == 4:
                marker_pose = m.pose.pose
                pos = marker_pose.position
                ori = marker_pose.orientation
                rospy.loginfo("marker[%d] position (x,y,z) = (%3.3f: %3.3f: %3.3f), orientation (x,y,z,w) = (%3.3f: %3.3f: %3.3f: %3.3f)" 
                              % (marker_id, pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w))

        return

if __name__ == '__main__':
    rospy.init_node('ar_pose_subscriber')
    ARPoseSubscriber()
    
    rospy.spin()

ここでは、3x3の九つマーカーが印刷されたものを使っているので、センターにあるmarker_id==4のものだけ、loginfoに出力するようにしている。

$ chmod 755 ar_pose_subscriber.py

として、実行可能にしておく。

つぎに、launchファイルに下のように追加する。

  <node name="ar_pose_subscriber" pkg="camera_eval" type="ar_pose_subscriber.py"/>

パッケージ名は、先ほどのコードが置いてある自作のパッケージ名として、実行可能にしたpythonファイルを指定する。

別の端末を立ち上げて、

rqt_console

とすれば、出力したloginfoがどんどん発行されて流れて行くのが見れる。

ひとまず、位置・姿勢の情報がとれた。厳密な値の比較をしていないが、カメラからマーカーを離すとzの値が増加するし、カメラに対してマーカーを右に動かすとプラスに、左に動かすとマイナスに値が変化する様子が見れた。

tfトピックのデータ取得（失敗？）

それでは次に、/tfトピックでもできるかやってみよう。

（2018/4/12追記）

/ar_pose_markerの土岐と同様に、/tfのメッセージの型も分かったので、

tf_subscriber.py

import rospy
import tf
import tf2_ros

from tf2_msgs.msg import TFMessage

class TfSubscriber(object):
    def __init__(self):
    
        self.sub = rospy.Subscriber('/ar_track_alvar/tf', TFMessage, self.callback_tf)

    def callback_tf(self, markers):

        for m in markers.transforms:
            marker_pose = m.transform
            tra = marker_pose.translation
            rot = marker_pose.rotation
            rospy.loginfo("position (x,y,z) = (%3.3f: %3.3f: %3.3f), orientation (x,y,z,w) = (%3.3f: %3.3f: %3.3f: %3.3f)" 
                          % (tra.x, tra.y, tra.z, rot.x, rot.y, rot.z, rot.w))

if __name__ == '__main__':
    rospy.init_node('tf_subscriber')
    TfSubscriber()
    
    rospy.spin()

としてみたのだが、rqt_consoleでloginfoに出てこなかった。launchしたときに特にエラー表示もないし、rqt_graphで見てもノードが立ってたり、/tfを受け取れている様子なんだけどね。

空飛ぶロボットのつくりかた ROStfのお勉強
うごくものづくりのために 座標変換パッケージ "tf"の使い方
MyEnigma 世界で一番簡単なtfの使い方
の情報を見てみると、topicのsubscribeではなくて、tfのlistenerを使う感じ？

tf（モジュール名とトピック名が同じだから、/tfってしないと分かりづらいな）のことをもっと勉強しよう。