Pepperめも ～障害物よけ移動～

#ペッパー君に障害物よけさて移動させてみたい
##なぜ？

• 早期レンタルでおうちにペッパー君がいる。
• でも、うちは、そこまで狭くはないけど、ペッパー君は慎重なので、たぶん半径0.7mくらいの範囲に障害物があるとそれ以上そっちには移動したがらないみたい。
• 日本の住宅事情的に結構それじゃあ自由に動けない！

##ということで…

• ひとまず一番手っ取り早いところから、お試しがてら障害物を判断したうえで移動させてみることに。
• ペッパー君には、３つのレーザーセンサーが付いている。
• NaoQiドキュメントに乗ってるレーザーセンサーではないようなので注意(説明はそれを理解した上じゃないと意味なさそう）Nao用レーザーセンサヘッドのセンサはググると１５万円する奴で、たぶん性能はペッパー君のほうが性能控えめっぽい。
• という事で、まるでカルネージハートで遊んでた時のような感覚で、すごくショボイプログラムをさくっと（と言っても範囲手さぐりなので時間かかったけど）書いてみた。

コード

• ipython用です
• ipythonは、windowsでも、公式のインストーラーで放り込んで、naoqiのパッケージもwindows用のを放り込んで、あとはpipというのを入れてpip install XXX とやったりするだけでさくっと導入できたのでなかなかお手軽。

%matplotlib inline
from naoqi import ALProxy
from pprint import pprint
import time, math
import pylab as pl
from IPython import display

IP = "192.168.11.7"
PORT = 9559

laser = ALProxy("ALLaser", IP, PORT)
memory = ALProxy("ALMemory", IP, PORT)
motion = ALProxy("ALMotion", IP, PORT)
laser.laserON()

baseName = "Device/SubDeviceList/Platform/LaserSensor/"
devNames = ["Front/","Left/","Right/"] 
rad = 0
moveRad = 0.125/2
#motion.post.moveTo(0, 0, 8)
for loopNow in range(100):
    #pl.cla()
    pl.xlim(-1,+1)
    pl.ylim(-1,+1)
    for jj in range(0,4):
        baseRot = 0
        for devName in devNames:
            for i in range(1,16):
                x = memory.getData(baseName+devName+"Horizontal/Seg%02d/X/Sensor/Value"%i)
                y = memory.getData(baseName+devName+"Horizontal/Seg%02d/Y/Sensor/Value"%i)
                #pprint(devName+"Horizontal/Seg%02d"%i+str((x,y)))
                #pl.plot(x,y,'o')
                r = rad + baseRot
                rx = x * math.cos(r) - y * math.sin(r) 
                ry = x * math.sin(r) + y * math.cos(r)
                pl.plot(rx,ry,'o')
            baseRot = baseRot + 2*math.pi/3
    motion.moveTo(0, 0, 2*math.pi*moveRad)
    rad += 2*math.pi* moveRad
    display.clear_output(wait=True)
    display.display(pl.gcf())

なにやってるの？

• レーザーセンサの値をとってきて、最短っぽいところを適当に左右の範囲を制限しつつ、調べて、0.7m以内だったら回転…ついでに左右に動けるようなら動いてみる、みたいなことをやってるだけ。
• あとはレーザーセンサーの値をグラフに表示してみたりしてるだけ。

今後の展開

• 意外にこれだけでもなかなか面白かった。カルネージハート的な面白さ。
• レーザーセンサ最短距離調べるブロックとか作ればコレグラフでも遊べるものになりそう。
• 今はかなり遅いので、そこを何とかしてみたい。モーション終わった後、再度動き出すまでに時間がかかるのは何とかできるかなぁ
• メモリの値をとってくるところも妙に遅いので纏めてとってくる方法とか調べよう。
• ペッパー君のセンサーには、超音波距離センサ（だと思う）などもあるので活用してみたい。
• 一番使いたいのは、デプスセンサ(Kinectと同じ)と２つのカメラ。これの画像処理できれば、2Dカメラの視差とデプスセンサで地面と障害物を見分けて、通れる道をスキャンしてからパスを作って移動とか出来そう。カルネージハート的感覚では物凄く燃える展開な予感。
• カルネージハートでは最強のプログラム（アルゴリズム）作って、いろんな環境でうまく動くのやら特化したのやらを競って遊んでたけど、あんな感じのオモチャになりそうと考えたらすごく楽しそうに思えてきた。
• …限られた性能の中でやる雰囲気がまさに。Pepper君、処理能力的な部分では結構リーズナブル。サーボとか筐体とかがきっとバカ
  高いのだろう。この形のボディだけでも相当するんだろうなぁ

なう

• 今は、画像処理やるなら噂に聞くROSを使ってみたいと試行錯誤中。Naoqiをラップしてノードにするものがあったので、ROSさえ動くようになれば色々出来そうで楽しみ。