概要

　本記事では、低コストのステレオカメラを搭載した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/

構成

ハードウェア

ロボット
- GoPiGo3
- Raspberry Pi3 Model B
- ステレオカメラ(ELP 720P Dual Lens Camera)
- Wifiアダプタ
- 11.1Vリチウムポリマーバッテリ
- USBハブ

※Raspberry Pi3 Model BのオンボードWifiでは、画像の送信に十分なスループットが出ないため、Wifiアダプタを利用しています。

ホストPC

項目	値
CPU	Core i7-7700
メモリ	8GB

OS

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

システム構成

今回構築したシステムは以下の図のようになっています。

ROSの概要

　ROSは、ロボット開発のためのミドルウェアで、様々なパッケージやツールを提供しています。ROSは、Navigationを行うためのパッケージ群であるNavigation Stackを提供しています。
Navigation Stackは以下の図のような構成になっています。

Navigation Stackの詳細は、以下の記事をご覧ください。
https://qiita.com/MoriKen/items/74cbff9198f82f2b7ea8

ホストPCのセットアップ

ROSのインストール

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

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のワークスペースを作成します。

ワークスペース作成

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アドレスに書き換えてください。

~/.bashrc

export ROS_MASTER_URI=http://<ホストPCのIPアドレス>:11311
export ROS_IP=<ホストPCのIPアドレス>

以下のコマンドで変更を適用します。

source ~/.bashrc

ROSパッケージのインストール

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

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します。

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のインストールを行います。

sudo apt-get install ntp

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

/etc/ntp.conf

tinker panic 0
server ntp.nict.jp iburst

GoPiGo3のセットアップ

ROSのインストール

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

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のワークスペースを作成します。

ワークスペース作成

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アドレスに書き換えてください。

~/.bashrc

export ROS_MASTER_URI=http://<ホストPCのIPアドレス>:11311
export ROS_IP=<GoPiGo3のIPアドレス>

以下のコマンドで変更を適用します。

source ~/.bashrc

gopigo3_nodeのインストール

以下のコマンドを実行し、GoPiGo3のドライバを導入します。

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のインストールを行います。

sudo apt-get install ntp

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

/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の実行

以下の図のように、ロボットの前方に障害物を設置した環境でコマンドを実行しました。

ホストPCで実行するコマンド

まず、以下のコマンドを実行し、Navigation Stackとrvizを立ち上げます。

roslaunch gopigo3_navigation navigation_view.launch

上記のコマンドを実行すると以下の図のようなRvizが表示されます。

次に、別のターミナルで以下のコマンドを実行し、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を行うことが出来ます。

ゴールの指定方法

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はロボットから見た相対的な座標ではなく、マップ上の絶対的な座標です。

ゴールを指定するプログラム

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

#!/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公開を予定しています。

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

概要

目次

構成

ハードウェア

OS

システム構成

ROSの概要

ホストPCのセットアップ

ROSのインストール

ホストPCのROSネットワーク設定

ROSパッケージのインストール

NTPのセットアップ

GoPiGo3のセットアップ

ROSのインストール

GoPiGo3のROSネットワーク設定

gopigo3_nodeのインストール

ステレオカメラパッケージのインストール

NTPのセットアップ

Visual Navigationの実行

ホストPCで実行するコマンド

GoPiGo3で実行するコマンド

ゴールの指定方法

コマンドによるゴールの指定

ゴールを指定するプログラム

告知

低コスト自律移動ロボットの開発

MAGI(Middleware for Artificial General Intelligence)