ステレオカメラを搭載したGoPiGo3でROSを用いたVisual Navigationを行う

# 概要
　本記事では、低コストのステレオカメラを搭載したGoPiGo3に、ROSのNavigation Stackを導入することで、低コストで自律移動ロボットを構築する方法について説明します。
　通常、LIDARと呼ばれるレーザ測距センサを利用してNavigationを行いますが、レーザ測距センサの代わりにステレオカメラを利用することで、より低コストで自律移動ロボットを構築することが出来ます。
　ステレオカメラで自律移動を行うために、Visual SLAMの1つであるRTAB-MAPを利用しました。

　GoPiGo3のセットアップを行っていない場合、以下の記事を参考にして、I2CボードのセットアップやRaspberry Pi3 Model BにUbuntu mate 16.04をインストールしてください。
https://qiita.com/cut0ter/items/0326f5dee89456e47001
http://deviceplus.jp/hobby/raspberrypi_entry_048/

# 目次
1. 構成
1. ホストPCのセットアップ
1. GoPiGo3のセットアップ
1. 実行 
1. ゴールの指定方法

#構成

### ハードウェア
- ロボット
 - GoPiGo3
 - Raspberry Pi3 Model B
 - ステレオカメラ(ELP 720P Dual Lens Camera)
 - Wifiアダプタ
 - 11.1Vリチウムポリマーバッテリ
 - USBハブ
  
※Raspberry Pi3 Model BのオンボードWifiでは、画像の送信に十分なスループットが出ないため、Wifiアダプタを利用しています。

 <img width="700" alt="system.jpeg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/311918/5abafaf8-e4d3-8c8e-34cf-45028381a949.jpeg">

- ホストPC
 
| 項目 | 値 |
|:-:|:-:|
| CPU | Core i7-7700 |
| メモリ | 8GB |


### OS
- Raspberry Pi3 Model B
 - Ubuntu mate 16.04
- ホストPC
 - Ubuntu 16.04 (Xenial)

### システム構成
今回構築したシステムは以下の図のようになっています。
<img width="600" alt="system.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/311918/16330311-2074-28a4-f10f-c06341897605.png">

# ROSの概要
　ROSは、ロボット開発のためのミドルウェアで、様々なパッケージやツールを提供しています。ROSは、Navigationを行うためのパッケージ群であるNavigation Stackを提供しています。
Navigation Stackは以下の図のような構成になっています。
<img width="600" alt="system.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/311918/71605ca0-b411-cbd4-bbce-bb48ee869800.png">
Navigation Stackの詳細は、以下の記事をご覧ください。
https://qiita.com/MoriKen/items/74cbff9198f82f2b7ea8


# ホストPCのセットアップ
### ROSのインストール
ホストPC上で、以下のコマンドを実行しROSをインストールします。

```bash:ROSのインストール
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
sudo apt-get update
sudo apt-get install ros-kinetic-desktop
sudo rosdep init
rosdep update
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt-get install python-rosinstall
```

以下のコマンドを実行し、ROSのワークスペースを作成します。

```bash:ワークスペース作成
mkdir -p ~/catkin_ws/src  
cd ~/catkin_ws/src  
catkin_init_workspace  
cd ~/catkin_ws/  
catkin_make  
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
```

### ホストPCのROSネットワーク設定
`~/.bashrc` のROS_MASTER_URIとROS_IPをホストPCのIPアドレスに書き換えます。
ROS_MASTER_URIとROS_IPが記述されていない場合、以下を追記しホストPCのIPアドレスに書き換えてください。

```bash:~/.bashrc
export ROS_MASTER_URI=http://<ホストPCのIPアドレス>:11311
export ROS_IP=<ホストPCのIPアドレス>
```
以下のコマンドで変更を適用します。

```
source ~/.bashrc
```

### ROSパッケージのインストール
以下のコマンドを実行し、必要なROSパッケージをインストールします。

```bash:ROSパッケージのインストール
sudo apt-get install -y \
    ros-kinetic-compressed-image-transport \
    ros-kinetic-rtabmap-ros \
    ros-kinetic-robot-localization \
    ros-kinetic-navigation \
    ros-kinetic-robot-state-publisher \
    ros-kinetic-joint-state-publisher \
    ros-kinetic-stereo-image-proc \
    ros-kinetic-pointcloud-to-laserscan \
    ros-kinetic-dynamic-reconfigure \
    ros-kinetic-rqt-reconfigure \
    ros-kinetic-camera-calibration \
    ros-kinetic-image-view \
    ros-kinetic-uvc-camera \
    ros-kinetic-image-transport \
    ros-kinetic-image-transport-plugins \
```

次に、以下のコマンドを実行し、Navigationのためのパッケージをbuildします。

```bash:
cd ~/catkin_ws/src/
git clone https://github.com/sbtit/gopigo3_navigation
cp -rf ~/catkin_ws/src/gopigo3_navigation/pointcloud_to_laserscan/ ~/catkin_ws/src/
cd ~/catkin_ws/
catkin_make
```

### NTPのセットアップ
以下のコマンドを実行し、NTPのインストールを行います。

```bash:
sudo apt-get install ntp
```
次に、NTPの設定を行います。`/etc/ntp.conf`の2行目辺りに以下の内容を追記してください。

```bash:/etc/ntp.conf
tinker panic 0
server ntp.nict.jp iburst
```

# GoPiGo3のセットアップ
### ROSのインストール
ホストPCと同様に、GoPiGo3上で以下のコマンドを実行し、ROSをインストールします。

```bash:ROSのインストール
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
sudo apt-get update
sudo apt-get install ros-kinetic-desktop
sudo rosdep init
rosdep update
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt-get install python-rosinstall
```

以下のコマンドを実行し、ROSのワークスペースを作成します。

```bash:ワークスペース作成
mkdir -p ~/catkin_ws/src  
cd ~/catkin_ws/src  
catkin_init_workspace  
cd ~/catkin_ws/  
catkin_make  
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
```

### GoPiGo3のROSネットワーク設定
`~/.bashrc` のROS_MASTER_URIとROS_IPをホストPCのIPアドレスとGoPiGo3のIPアドレスに書き換えます。
ROS_MASTER_URIとROS_IPが記述されていない場合、以下を追記しホストPCのIPアドレスとGoPiGo3のIPアドレスに書き換えてください。

```bash:~/.bashrc
export ROS_MASTER_URI=http://<ホストPCのIPアドレス>:11311
export ROS_IP=<GoPiGo3のIPアドレス>
```
以下のコマンドで変更を適用します。

```
source ~/.bashrc
```

### gopigo3_nodeのインストール
以下のコマンドを実行し、GoPiGo3のドライバを導入します。

```bash:
cd ~/catkin_ws/src/
git clone https://github.com/ros-gopigo/gopigo3_node.git
```

`~/catkin_ws/src/gopigo3_node/src/gopigo3_diver.py`を以下のように書き換えます。

`line 106 "odometry" -> "odom"`
`line 259    world -> odom`
`line 259 gopigo -> base_link`

### ステレオカメラパッケージのインストール
以下のコマンドを実行し、ステレオカメラ画像を送信するためのパッケージをインストールします。

```
sudo apt-get install ros-kinetic-camera-calibration
sudo apt-get install ros-kinetic-uvc-camera
```

次に、以下のコマンドを実行し、ELPステレオカメラ用のキャリブレーションファイルをダウンロードします。

```
cd ~/.ros/
git clone https://github.com/sbtit/gopigo3_robot.git
```

以下のコマンドを実行し、buildします。

```
cd ~/catkin_ws/
catkin_make
```

### NTPのセットアップ
以下のコマンドを実行し、NTPのインストールを行います。

```bash:
sudo apt-get install ntp
```
次に、NTPの設定を行います。`/etc/ntp.conf`の2行目辺りに以下の内容を追記してください。

```bash:/etc/ntp.conf
tinker panic 0
server ntp.nict.jp iburst
```

時刻のズレがひどい場合は、ntpdateを利用します。
以下のコマンドでntpdateをインストールします。

```
sudo apt-get install ntpdate
```

次に、以下のコマンドを実行し、ntpdateを日本の標準時刻に設定します。

```
ntpdate ntp.nict.jp
```

# Visual Navigationの実行
以下の図のように、ロボットの前方に障害物を設置した環境でコマンドを実行しました。
<img width="700" alt="jpeg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/311918/abc61cc0-10d4-0a1b-ae68-400de8cf1a9f.jpeg">

### ホストPCで実行するコマンド
まず、以下のコマンドを実行し、Navigation Stackとrvizを立ち上げます。

```
roslaunch gopigo3_navigation navigation_view.launch
```
上記のコマンドを実行すると以下の図のようなRvizが表示されます。
<img width="836" alt="Rviz.PNG" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/311918/521ab688-a607-188f-b69a-1e698f67779e.png">



次に、別のターミナルで以下のコマンドを実行し、ELP 720P用のキャリブレーションファイルを読み込みます。
`/home/username/catkin_ws/...`のusernameは、自分の環境に合わせて変更してください。

```
rosrun dynamic_reconfigure dynparam load stereo/stereo_image_proc /home/username/catkin_ws/src/gopigo3_navigation/setting5.yaml
```
　正常に読み込むことができた場合、何も出力されずにプロセスが終了するため、続けて同じターミナルで以下のコマンドを実行し、深度情報を点群情報に変換するノードを起動します。

```
cd ~/catkin_ws/src/pointcloud_to_laserscan/launch
roslaunch p2l.launch
```

### GoPiGo3で実行するコマンド
　GoPiGo3で以下のコマンドを実行し、GoPiGo3のドライバとステレオカメラを起動します。
　以下のコマンドの`left/camera_info_url`と`right/camera_info_url`の`///home/username/.ROS/...`の部分は、自分の環境に合わせてusernameを変更してください。

```
ROS_NAMESPACE=stereo rosrun uvc_camera uvc_stereo_node _left/device:=/dev/video0 _right/device:=/dev/video1 _fps:=15 _width:=320 _height:=240 _left/camera_info_url:=file:///home/username/.ros/camera_info/left.yaml _right/camera_info_url:=file:///home/username/.ros/camera_info/right.yaml & roslaunch gopigo3_node gopigo3.launch
```
　障害物を設置した環境で上記のコマンドを実行すると、Rvizに以下の図のような障害物情報が反映されます。
後は、ゴールの指定を行うことで、Navigationを行うことが出来ます。
<img width="700" alt="Rviz2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/311918/65c52461-f36a-4112-0d9f-cdba83f80196.png">


# ゴールの指定方法
LIDARを利用するNavigationと同様に、ロボットを目的地まで移動させることが可能です。

### コマンドによるゴールの指定 
以下のコマンドを実行することで、ロボットのゴールを指定することが出来ます。

```
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{ header: { frame_id: "map" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
```

position内のパラメータを変更しすることで、x,y座標を指定できます。
また、orientation内のパラメータを変更しロボットの方位指定が出来ます。

※positionはロボットから見た相対的な座標ではなく、マップ上の絶対的な座標です。

### ゴールを指定するプログラム
ゴールを指定するプログラムの一例を以下に示します。

```python:
#!/usr/bin/env python
#license removed for brevity

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 
def movebase_client():
  
    client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
    client.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = 0.5
    goal.target_pose.pose.position.y = 0.3
    goal.target_pose.pose.orientation.z = 0.0

    client.send_goal(goal)
    wait = client.wait_for_result()
    if not wait:
        rospy.logerr("server not available!")
        rospy.signal_shutdown("server not available!")
    else:
        return client.get_result()

if __name__ == '__main__':
    try:
        rospy.init_node('test_nav_py')
        result = movebase_client()
        if result:
            rospy.loginfo("Goal!")
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation test finished.")
```

`goal.target_pose.pose.position.x`や
`goal.target_pose.pose.position.y`の値を変更することで、ゴールの座標が設定できます。

# 告知
　我々は、「生物的知能」を実現するロボットの制御機構を開発することを目的とした**iOt Project**を実施しています。

### 低コスト自律移動ロボットの開発
　本記事で取り上げたVisual Navigationだけでなく、低コストLIDARを利用したNavigation機構の構築を行いました。
以下のGitHubを参考にして、LIDARを利用したNavigation機構を構築することが出来ます。
https://github.com/iot-magi/gopigo3_navigation

### MAGI(Middleware for Artificial General Intelligence)
 iOt Projectでは、AIとIoTを組み合わせたAIoTシステムの開発を容易にするミドルウェアである**MAGI**の開発を行っています。
MAGIは、小型ロボットや玩具、家電などの身の回りのデバイスの制御を目的としており、これらとkubernetesベースのクラウドを接続することにより、AIoTシステムの開発に必要な、広範囲の機能を提供します。
具体的には以下の機能を提供します。

* クラウド上のdockerとして実行されるROSモジュールや深層学習プログラムとデバイスを接続する機能
* MAGIが提供する開発環境からdocker形式のAIアルゴリズムを実行する機能
* 必要なリソースを統合的に管理する機能
    * ロボットの接続、実行、停止を制御する機能
* 上記の機能をブラウザから簡単に操作できるGUI機構

今年度中にOSSとしてMAGIのv1公開を予定しています。