※この記事は、Pepper NAOqi OS 2.5.5 SLAM機能を試してみたの続きです。

移動可能範囲の探索とデータ保存

まずはPepperが移動できる範囲を探索します。

explore(float radius)

パラメータ：radius – 最大の探索範囲をメートルで指定
戻り値：0 – 正常、エラー時はError code表に従った値が返されます。 エラーコード表

このメソッドを実行するとPepperはパラメータで指定された最大値の範囲で自身が移動できる範囲の探索を始めます。探索中Pepperは辺りをうろうろします。
探索が終了すると、探索で得たデータをディスクに保存します。

saveExploration()

戻り値：探索で作成された地図データファイル（.explo）のパスを返します。

このメソッドで保存されたデータは後に地図を作成するときに使用します。戻り値で地図データファイルのパスが返されますが、このサンプルでは保存したファイルを操作する処理はしません。

指定場所への移動処理

Pepperが移動できる範囲を認識したあと、その範囲内で指定した場所へ移動させることができます。指定方法はX方向、Y方向、Theta方向の距離をパラメータで指定します。
Thetaに関しては、パラメータにはありますが、現時点では動作しません。なので、Pepperの向きを指定したい場合は、ALMotion.MoveToを使用します。

startLocalization()

戻り値：なし

stopLocalization()

戻り値：なし

navigateToInMap(float target)

パラメータ：target – [x, y, theta]の書式で移動させたい方向を指定します。
例）navigateToInMap([0., 0., 0.])
戻り値：0 – 正常、エラー時はError code表に従った値が返されます。（Error code表）

Pepperを移動させたい時はnavigateToInMap()を使いますが、このメソッドは上記のstartLocalization, stopLocalizationで括って使用します。つまり以下のようにまとめて使用します。

使用例）
startLocalization()
navigateToInMap([0., 0., 0.])
stopLocalization()

保存した移動可能範囲の地図データ取得と地図の作成

最後に 移動可能範囲の探索とデータ保存で作成した探索データを取得して地図を作成します。探索データの取得はNAOqi2.5.5で追加されたSLAM機能のAPIを使いますが、地図の作成とタブレットへの表示は既存のAPIを使用します。

getMetricalMap()

戻り値：現在の探索で保存したデータを地図データとして返します。決められたフォーマット[mpp, width, height, [originOffsetX, originOffsetY], [pxVal, …]]のデータが配列として返されます。

mpp: 地図の解像度で、１pixelあたりのメートル数です
width, height: イメージのサイズをpixel単位で表したものです
originOffsetY, originOffsetY: 地図のpixel(0, 0)のオフセットです
pxVal: 0 – 100の間のpixel値のバッファです（と、APIドキュメントで記載されてますが、具体的なデータの意味は不明・・）

全体のコード

    import numpy
    import Image

    self.framemanager = ALProxy(“ALFrameManager”)
    self.navigation_service = ALProxy(“ALNavigation”)

    # 10メートルの範囲を探索
    radius = 10.0
    error_code = self.navigation_service.explore(radius)
    if error_code != 0:
        print "Exploration failed."
        return
    # 探索した地図データをディスクに保存
    path = self.navigation_service.saveExploration()
    self.logger.info( "Exploration saved at path: \"" + path + "\"" )
    # 地図データ上で自己位置からの移動準備
    self.navigation_service.startLocalization()
    # 最初のポジションに戻る
    self.navigation_service.navigateToInMap([0., 0., 0.])
    # 移動完了
    self.navigation_service.stopLocalization()
    # タブレットに地図を表示するため探索した地図データを取得
    result_map = self.navigation_service.getMetricalMap()

    # 以降は既存のAPIを使って地図ファイルを作成
    path = os.path.join(self.framemanager.getBehaviorPath(self.behaviorId),”../html/img/”)
    writepath = path + “map.jpg”
    map_width = result_map[1]
    map_height = result_map[2]
    img = numpy.array(result_map[4]).reshape(map_width, map_height)
    img = (100 - img) * 2.55 # from 0..100 to 255..0
    img = numpy.array(img, numpy.uint8)
    Image.frombuffer('L', (map_width, map_height), img, 'raw', 'L', 0, 1).save(writepath,"JPEG")

このあとはShowImageボックスで作成した地図を表示します。

まとめ

SLAM機能を使ってみた所、自己位置の探索および移動可能範囲を特定し、地図を作成することは想定どおりの動作でしたが、navigateToInMapを使って指定位置に移動が想定どおりに動作しないことが何度かありました。
4月4日にNAO qi OS 2.5.5 の正式版がリリースされるということで、β版で特に目立っていた、動作の不安定さが改善されることを期待したいと思います。

謝辞

この7日間のSLAMの検証をするにあたって、アトリエ秋葉原のスタッフには大変お世話になりました。
今までの開発ではなかったPepperが自ら動きだすダイナミックな検証にはなりましたが、検証スペースの確保や2.5.5対応Pepperの手配など、アトリエスタッフの強力なサポートのおかげでスムーズな検証を行うことができました。
この記事を通じて御礼申し上げます。
ありがとうございました！