kinect から 距離画像を取ってくるROSパッケージ

kinect の記事を書いたらアクセスが増えたので追加してみます。
今回は、ROS 経由でkinect の距離画像を取ってくる方法です。



(1) ROS パッケージを作成
ここでは kinectTest という名前にしてみます。

roscreate-pkg kinectTest rospy roscpp std_msgs

この辺は、 ROS のチュートリアル (http://www.ros.org/wiki/ROS/StartGuide) にあります。



(2) ROS_PACKAGE_PATH に kinectTest へのパスを通す。
ディレクトリが /opt/kinectTest だとすると、
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/opt/kinectTest



(3) node 作成。
kinectTest ディレクトリ以下に node ディレクトリを作成し、
以下のスクリプトをおく。
そして、実行権限を与える。  (chmod +x ファイル名)

#!/usr/bin/env python


import roslib
roslib.load_manifest('kinectTest')
roslib.load_manifest('openni_camera')
import rospy
from sensor_msgs.msg import PointCloud2
import numpy
import pylab
import time




def callback(data):
    resolution = (data.height, data.width)
    # 3D position for each pixel
    img = numpy.fromstring(data.data, numpy.float32)
    x = img[0::4].reshape(resolution)
    y = img[1::4].reshape(resolution)
    z = img[2::4].reshape(resolution)
    
    # show them
    print time.asctime()
    pylab.clf()
    pylab.imshow(numpy.flipud(z), interpolation="nearest",
                 vmax=3, vmin=0)
    pylab.xlim([0, data.width])
    pylab.ylim([0, data.height])
    pylab.colorbar()
    pylab.draw()




def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber('/camera/depth/points2',
                     PointCloud2, callback)
    rospy.spin()


if __name__ == "__main__":
    pylab.ion()
    listener()

(4) 走らせる。

まず kinect から画像を取得する。

roslaunch openni_camera openni_kinect.launch 

次に、本プログラムを実行

rosrun kinectTest showDepth.py

すると、距離画像がリアルタイムで表示されます。

やや遅いのは pylab.draw を使っているからでしょう。

--- 

これで z に距離画像の2次元配列を得ることができるので、

色々やると何か新しいことができそうです。