Pepperめも ～ROSにチャレンジ～

# ROS？
- http://wiki.ros.org/nao
- 世界で一番普及してるロボットOS。OSというよりフレームワーク。
- 色々整ってて凄そう。特に可視化周り。
- Ubuntuでのみ動く。他の環境は茨の道らしい。
- 詳しくはQiitaに記事がいくつもある。

# NaoQi?
- Pepper君のフレームワーク。どちらかというとOSではない。
- OSという意味ではLinuxが標準。
- シーケンスのある処理に向いてるらしい。
- リモートでもコレグラフのPythonでやってる事と同様な操作を利用できる。

# ROS+NaoQi
- Naoqiをリモート前提でROSのノードにする環境があるらしい
- ROS上の各種可視化ツールは、NaoQiのモニターの比じゃないくらい凄そう。
- いろいろ探すと[KinectでSLAM](http://ros-robot.blogspot.jp/2011/04/rgbdslam.html)とか魅力的なのが見つかる。
- と、いう事で、面白そうなので、全くの初心者(Linux含む)だけどチャレンジ

# My環境
- ハード: VaidDuo 13 
- OS: VirtualBox
 - ホストOS windows 8.1
 - ゲストOS [rosHydro.torrent](http://nootrix.com/downloads/#RosVM) ※Indigoという最新のイメージは起動しなかったので諦めた 

# いきなりだけど失敗
- 苦節、↑書いてから約一か月…（OculusJamに浮気したりしまくってたけど）
- ROS入れること自体は成功、画像得たり色々動かせた、けれど…一番やりたかったSLAM。その中でもLSD-SLAMというのがお手軽＆凄そうでよさげ。でも、Hydro未対応！
 - ずいぶん時間かけたけれど、結局makeがうまくいかなかったので、すべて捨てて素直にやり直すことに！
 - (opencvのバージョン違うだって？ opencv！make！途中でエラー！ソースが膨大でもうやだcmakeってなんなのさ…みたいな)

# Ubuntuからヤル
- ROS Indigo入りイメージが動かなかったので諦めてたけど、インストール自体はubuntu&Linux初心者でもそこそこ敷居が低い事に気付いたのでそこからやることに。
- ubuntu 14.04 のイメージをダウンロード  https://www.ubuntulinux.jp/download/ja-remix-vhd
- VT-xが無効といわれたのでググるとVaioは標準では無効らしいので変更。
- 起動！
 - …あ…
 - ジーザス！起動したけど何分待っても最初の画面から先に進まない！
 - じっくり設定を再チェック… VirtualBoxの設定がUbuntu32bitだから？ 
 - 64bitに直したら動いた！

# ROSインストール
- 以下のページが超参考になる
 - http://watada.info/rosguide/01.html
 - http://wiki.ros.org/indigo/Installation/Ubuntu
- 手順といっても、普通は 6コマンド打つだけで完了
 - linuxMemo: sourceコマンドとは。ファイルに書かれたコマンドを実行するコマンド
- トラぶらなければDL時間含めて小一時間で終わりそう(ネット環境とかPCの処理速度によるかも)

# パッケージのビルド環境整える
- その前にスナップショットとっておこう。

## まずはcatkin(ROSの新しい方のビルドシステム)でパッケージをビルドする環境を整える

以下を実行。

```
sudo apt-get install python-wstool
cd ~
mkdir -p ros_catkin_ws/indigo/src
cd ros_catkin_ws/indigo/src
wstool init .
cd ..
catkin_make
echo "source $HOME/ros_catkin_ws/indigo/devel/setup.bash" >> ~/.bashrc
```
- 詳しくはこちらの方の投稿を読めば完璧（というか↑はIndigoに名前を変えてコピペしてちょっと加えただけ）[コピペ元リンク(ros_tutorial)](http://qiita.com/garaemon/items/e5554d6c03a7d475a682#3-2)
- こちらは本家のチュートリアル http://wiki.ros.org/catkin/Tutorials/create_a_workspace

##つぎはrosbuild(古いビルドシステム)でパッケージをビルドする環境を整える

以下をじっこう。

```
sudo apt-get install python-rosinstall
cd ~
mkdir -p ros/indigo
cd ros/indigo
rosws init . ~/ros_catkin_ws/indigo/devel
echo "source $HOME/ros/indigo/setup.bash" >> ~/.bashrc
```

- 詳しくはこちらの方の投稿を[コピペ元リンク(ros_tutorial)](http://qiita.com/garaemon/items/e5554d6c03a7d475a682#3-2)
- 中身を詳しく理解してないけどcatkinのdevelにオーバーレイとやらをするのが重要？
- 本家のほうのリンク  http://wiki.ros.org/fuerte/Installation/Overlays

# Naoqiとか入れる ～基本のSDK～
- その前にスナップショットとっておこう！

```
mkdir ~/naoqi
```
- 作ったnaoqiフォルダに、アルデバランのページがら落とせるnaoqi-sdk-2.0.5.3-linux64.tar.gz を放り込んで展開。そのあと…

```
echo "export PYTHONPATH=~/naoqi/naoqi-sdk-2.0.5.3-linux64/lib:$PYTHONPATH" >> ~/.bashrc
```
- で第一段階完了。試しに実行してチェック(Ctrl-Cで終了)

```
~/naoqi/naoqi-sdk-2.0.5.3-linux64/naoqi
```

- 参考リンク 
 - http://wiki.ros.org/action/show/nao/Tutorials/Installation?action=show&redirect=nao%2FInstallation
 - 現在 nao から naoqi にリポジトリ名を変えている最中らしいので色々名前が錯綜、リンク切れも多数あるので混乱しないよう注意みたいデス

#Naoqiとか入れる ～PythonのSDK～
- さっき作っておいた naoqiフォルダに アルデバランから落とせる pynaoqi-python2.7-2.0.5.3-linux64.tar.gz を放り込んで展開
 - ↓を見ての通りだけど実際は場所はどこでもOK

```
echo "export PYTHONPATH=$PYTHONPATH:$HOME/naoqi/pynaoqi-python2.7-2.0.5.3-linux64/" >> ~/.bashrc
```

- ここでやったのはチュートリアルでは初心者向けと書かれた方法デス
 - http://wiki.ros.org/nao/Installation/remote
 - 難易度高いのはPepper君では無謀だと思う。
 - というより現状だとPepper君の中にROS入れるのは無保証っぽそう。やったところで素直に動かなさそうだし。

#Naoqiの周りを入れる
- ひとまず見つけたビルド済みなパッケージを放り込んでみる

```
sudo apt-get install ros-indigo-naoqi-driver 
sudo apt-get install ros-indigo-naoqi-sensors 
```
- 中身は大体Pythonで書かれたROSのノードなのでかなり参考になりそう
- 特にカメラとかはなかなかよいサンプル！
 - PythonのNaoのApiいじったことあれば大体想像で何やってるか読めるはず
 - https://github.com/ros-naoqi/naoqi_bridge/blob/master/naoqi_sensors/src/naoqi_sensors/naoqi_camera.py

#Ros meets pepper! ～Pepper君とつないでみる～
- まずはカメラ！
```
roslaunch naoqi_sensors camera.launch nao_ip:=<IPアドレス>
```
- カメラを扱うノードが起動 ↓こんな感じになる

```
> rosnode list
/camera
/rosout
/rqt_gui_py_node_4082

> rostopic list
/camera/camera_info
/camera/image_raw
/camera/parameter_descriptions
/camera/parameter_updates
/rosout
/rosout_agg
```
- そして映像をストリーミングで表示！
 - ついでに rqt_graph とか使うとグラフィカルにノードを表示できる。

```
rosrun image_view image_view image:=/camera/image_raw
```

![無題2.png](https://qiita-image-store.s3.amazonaws.com/0/32235/ebe0b69a-cd27-bd33-e382-e5cdc6f8f6fb.png)

- なお何かの初期設定のせいか画像真っ白だったので、純正のモニタアプリつかってカメラのパラメタ調整したらちゃんと映った。（ブライトネスの設定など） 
 - 追記 明るさの初期値128がPepper君のフロントカメラだと微妙っぽい

- メモ
 - ここで使ってるものは場所としては /opt/ros/indigo/share などなどに入っている

---

ひとまず、ここまでくれば後は流れる画像データなどをノード間で受け渡して色々と既存の処理を組み合わせて遊べる環境がお手元に！

# Let’s go SLAM 
- いざスラムの海原へ
- お試しするのは [**これ。lsd Slam！**](https://github.com/tum-vision/lsd_slam)
 - 単眼のカメラの動画像ベースで、かなりよいかんじなSLAMしてくれそうで、しかもソースコードがROSのノードとして公開されてるニクイ奴
 - [参考youtube](https://www.youtube.com/watch?v=SvsMuu5G-sg)
- あ、海原に行く前にまずは、スナップショットとっておこう！

以下を次々と実行！

依存関係のあるものザクッと入れて…

```
sudo apt-get install ros-indigo-libg2o ros-indigo-cv-bridge liblapack-dev libblas-dev freeglut3-dev libqglviewer-dev libsuitesparse-dev libx11-dev
```

作業用ディレクトリ作って…

```
cd ~/ros/indigo
mkdir package_dir
rosws set ~/ros/indigo/package_dir -t .
source ~/.bashrc
```

Gitでlsd-slamとってきてmake!

```
cd ~/ros/indigo/package_dir/
git clone https://github.com/tum-vision/lsd_slam.git lsd_slam
rosmake lsd_slam
```

メモ

```
※↓では作業用に隔離されたディレクトリを追加するコマンド。
rosws set ~/ros/indigo/package_dir -t .

引数の -t の意味はこんならしい。

-t WORKSPACE, --target-workspace=WORKSPACE
                        which workspace to use

※↓最後のこれは、前に追加しておいた更新用のやつ呼ぶためにやってるだけ
source ~/.bashrc

※↓追加結果などはこんなんで確認できるみたい。
(あとはroswsを引数無しで呼んでも情報出てくるようです）
echo $ROS_PACKAGE_PATH 
```

- 詳しくは、https://github.com/tum-vision/lsd_slam こちらのチュートリアルそのまんま！(roswsとかはあらかじめ設定しているのでそれに合わせてマス)

--- 

- よし！あっさり、できた！！やった！ながかった！
 - hydroのほうで入れるのは初心者には無謀だったみたい。
 - それにしてもこんなにROS環境が簡単に入るなんて…Linuxじゃないみたい！

# Go Go SLAM!
さっそく実行、苦節１ヶ月の成果が… どきどき

```
roslaunch naoqi_sensors camera.launch nao_ip:=<IPアドレス>
rosrun lsd_slam_core live_slam /image:=/camera/image_raw /camera_info:=/camera/camera_info
rosrun lsd_slam_viewer viewer
```
- **…動かないorz**

# Go Go SLAM...
- でもこんな時にはROS BAGが火を噴くぜ(もうテンションが変)

```
Download the Room Example Sequence and extract it.
```
- http://vmcremers8.informatik.tu-muenchen.de/lsd/LSD_room.bag.zip

```
rosrun lsd_slam_viewer viewer
rosrun lsd_slam_core live_slam image:=/image_raw camera_info:=/camera_info
rosbag play ~/LSD_room.bag
```
- 動いている状態のログデータからヒャッハーすれば不具合の場所なんてI CHI KO ROさ！

- ![無題.png](https://qiita-image-store.s3.amazonaws.com/0/32235/e0c78dc1-e16a-f9e4-3f1a-d3577eae9cd5.png)

- …こいつ動くぞ！（うごいてた）

# 海原だと思ったらラビリンス

じっくり確認！
```
WAITING for ROS camera calibration!
```
の状態で止まるっているらしいことを理解でヒャッハー

- 可能性１動いてるけどキャリブレーションできるだけの情報がない案
 - ペッパー君を動かしまくってみるぜ、ヒャッハー！
 - なんにも反応ないぜヒャッハー！ 

- 可能性２カメラ的にキャリブレーションきついんじゃ…案
 - じっくり英語を読んでみるぜヒャッハー
 - …わからないぜヒャ…
 - …camera_infoは、キャリブレーションにしか使ってないぜヒャッハーだって？
 - …直接指定する方法がヒャッハー？
 - ！！

# SLAMが立った！
```
~/ros/indigo/package_dir/lsd_slam/lsd_slam_core/calib/FOV_example_calib.cfg
```
を解像度だけ書き換えて作った
FOV_Pepper_calib.cfg

```
0.527334 0.827306 0.473568 0.499436 0.928161
320 240
crop
320 240
```
を引数に指定して…(カチャカチャ・・・ッターンッ！！)

```
rosrun lsd_slam_core live_slam /image:=/camera/image_raw _calib:=/home/haiatto/ros/indigo/package_dir/lsd_slam/lsd_slam_core/calib/FOV_Pepper_calib.cfg
```

![無題4.png](https://qiita-image-store.s3.amazonaws.com/0/32235/5571bd48-0cf2-6e89-34ea-4ef91d82d791.png)

- うごいた！

```
roslaunch naoqi_sensors camera.launch nao_ip:=<IPアドレス>
rosrun lsd_slam_core live_slam /image:=/camera/image_raw _calib:=/home/haiatto/ros/indigo/package_dir/lsd_slam/lsd_slam_core/calib/FOV_Pepper_calib.cfg
rosrun lsd_slam_viewer viewer
```

- ということでこれが勝利のカギ。

# せっかくなので

- カメラの解像度をVGAにして自分の環境で良い感じになるよう調整

```
roslaunch naoqi_sensors camera.launch nao_ip:=192.168.11.19 resolution:=2 brightness:=0 contrast:=32
```

- FOV適当だったので、資料からちゃんと設定したコンフィグファイルを作成

```
Sensor |X (m)|Y (m)|Z (m)|VFOV|HFOV|Rotation
CameraTop 0.0868 0.0000 0.1631 44.30° 57.20° No 
CameraBottom 0.0936 0.0000 0.0616 44.30° 57.20° rotationY(40.0*TO_RAD) 
```

```
fx/width fy/height cx/width cy/height d
in_width in_height
"crop" / "full" / "none" / "e1 e2 e3 e4 0"
out_width out_height
```

https://developers.google.com/project-tango/overview/intrinsics-extrinsics

```
import math
cx = 640/2 
cy = 480/2 
fx = cx/math.tan(57.20/2 * math.pi / 180)
fy = cy/math.tan(44.30/2 * math.pi / 180)
v0 = fx/640.0
v1 = fy/480.0
v2 = cx/640.0
v3 = cy/480.0
print v0,v1,v2,v3
```

FOV_Pepper_calib.cfg

```
0.917064872594 1.22827547556 0.5 0.5 0
640 480
none
640 480
```
- …あってるのだろうか微妙に自信なし
 - 一応 http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration で得た値と似ているのであってそう？
 - 追記 2015.2.9
 - これは間違ってるとおもわれます。続きの書きかけを投稿した方に言及してるところがあるのですが、確信がやっぱり持ててないので…キャリブレーションして値を得るのが確実かもです

- いざ実行！
 - うーん微妙な結果に…
 - 処理スピードかなぁ…
 - それとも首を動かしただけがダメなのかも？
 - でも、あとは楽しい調整だけ！

# 結果

- 時間がかかったけど初心者でも意外にすんなりROSで遊べることはわかった。
- Pepper君でより楽しく遊べる感じが凄くする！
- 次は、Lsd-Slamをまともに使えるようにして、マップと自分の位置を、ALWorldRepresentation か何かで送り込むノード作ってみるよてい！

# 以下纏めてない雑多なメモ

### ROS
- 参考リンク
 - [ros_tutorial](http://qiita.com/garaemon/items/e5554d6c03a7d475a682)
 - [ここの中身が最新のNAO関連らしい](https://github.com/ros-naoqi)
 - [凄そうな先人たちが居そうなgithubプロジェクト](https://github.com/jsk-ros-pkg/jsk_robot/issues/190)
 - http://wiki.ros.org/naoqi_driver
 - http://wiki.ros.org/nao/Tutorials
 - [ROSを俯瞰して理解したかったときにすごく役に立ったページ](http://www.rt-shop.jp/blog/archives/368)

- catkin
  - ROSの新しめなビルドシステム。植物の名前が由来。
  - 詳しそうな説明 http://d.hatena.ne.jp/meison_amsl/20131229/1388320084
  - ROSのページを見る上でビルドシステムは密接にかかわってそうな雰囲気
  - ドキュメント上の依存関係などの情報は主にここら辺から作られてそうな予感（？）
- パッケージ
 - ビルドできる単位。ビルド結果で１つ以上機能が出来上がるらしい。
- スタック
 - パッケージをまとめたもの。catkin移行時に廃止されたらしい。

- rpt_graph 
 - どんなノードがどんな情報をやり取りしてるか見えるので理解に役立ちそう

### Other
- /path/to/dir
 - Linux的な常套句らしい。そんなディレクトリがあるわけじゃいらしい(…なんて面妖な用語の使い方だろう…せめて<>で括って…と初心者キラーにはまる毎日makeみたいに何か裏技だらけのデフォルト設定があるのかと深読みしてしまった…)