Raspberry Pi 4 を使ったROS入門 第5回 RvizとSLAM体験

はじめに

この記事は研究室の学生の教育用に作成してゆきます．
現在はROS2がメジャーとなってきておりますが，まずは自習が出来るよう文献が豊富なROS1でROSの扱いに慣れてみます．
ROSは既に多くの文献があり，それだけでも自習可能ですが解説されているバージョンや環境が違い，初心者には躓きやすい内容だと思ったため，改めてまとめてみます．

前回はこちら
Raspberry Pi 4 を使ったROS入門　第4回　Gazeboを使ったシミュレーション
次回はこちら
Raspberry Pi 4 を使ったROS入門　第6回 ジョイスティックとカメラによるロボット操作

動作環境

Raspberry Piを使用します．(用意できるならUbuntu入りのPCが◎)
ROSはUbuntu上で動くソフトなのですが，設定の違いによってトラブルが起きやすいので，クリーンインストールしてから始めることをお勧めします．
ROSのバージョンはUbuntuのバージョンとリンクしてます．

本記事で使用する機器とバージョン

使用機器	Ubuntuバージョン	ROSバージョン
Raspberry Pi 4 Model B / 8GB	20.04	Noetic Ninjemys

※RAMが2GB，4GBだとビルドが途中で落ちることが多々ありました．
出来れば8GBが良いかもしれません．

1. ROSの可視化

　ROSにはロボット開発に役立つ様々なツールが用意されています．今回はRVizというツールを使って3次元データの可視化を行ってみます．
　ロボット内部の大量のデータを検証する場合，変数の中身の数値を可視化して調べることが非常に重要です．可視化によりシステム全体のデータの流れを俯瞰的に理解することが出来ます．またロボットの動作に問題が発生した場合，その原因を特定するためには再現性のある方法で実験を行う必要がありますが，実機を使った検証では再現性のある検証は難しく，実験の試行回数も増えてしまいます．
　RVizはロボットの3次元モデルや座標系，測定した3次元点群などを可視化するツールです．ロボットが認識している3次元空間の情報と共に，画像なども表示できます．表示可能なメッセージ型が多数用意されており，トピック名を指定する事で簡単に可視化できます．自分で新たな表示形式などのプラグインを追加することも可能です．

本来は実機のロボットで実施してみたいところですが，とりあえず今回はシミュレータと一緒に使ってみます．Roscoreを立ち上げた状態で別ターミナルで下記コマンドを入力するとRVizが起動します．

roscore

rosrun rviz rviz

今回はturtlebot3というロボットのモデルを使ってシミュレーションを行ってみます．
下記コマンドでパッケージをインストールします．

cd ~/catkin_ws/src
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
cd ~/catkin_ws && catkin_make

このロボットはBurger, Waffle, Waffle Pi の3種類あるのですが起動時に毎回指定する必要が出てきてしまいます．bashrcを書き換えておきます．下記コマンドでエディタを開きます．

gedit ~/.bashrc

最後の行に下記コマンドを追記して保存します．

export TURTLEBOT3_MODEL=burger

下記コマンドで反映させます．今後turtlebot3を読み込むときにモデルのエラーが出た場合は下記コマンドを再実行してください．

source ~/.bashrc

続いて下記コマンドでturtlebot3のシミュレーターをインストールします．

cd ~/catkin_ws/src
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
cd ~/catkin_ws && catkin_make

それでは下記コマンドでGazeboを起動してみます．

roslaunch turtlebot3_gazebo turtlebot3_world.launch

それではこのロボットが自律走行するプログラムを動かしてみます．下記コマンドで実行できます．

roslaunch turtlebot3_gazebo turtlebot3_simulation.launch

Gazebo上でロボットが障害物をよけながら進んでいることが分かります．
この状態でrvizを起動してみましょう．下記コマンドで実行できます．

roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch

そうするとrviz上でロボットがレーザー距離計で計測出来ている距離が可視化されます．

2. SLAMを体験

次に設定した目的地に障害物をよけながら進むSLAM(Simultaneous Localization and Mapping)を試してみます．

まずは環境の地図を作る必要が有ります．今立ち上げているプログラムを閉じて，下記コマンドを実行しSLAMのモジュールをインストールします．

cd ~/catkin_ws/src
git clone https://github.com/ros-perception/slam_gmapping.git
git clone https://github.com/ros-perception/openslam_gmapping.git
git clone https://github.com/ros-planning/navigation_msgs.git
git clone https://github.com/ros-planning/navigation.git
git clone https://github.com/ros/geometry2.git
sudo apt-get install ros-noetic-map-server

ビルドします．

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

続いて下記コマンドでGazeboを起動します．

roslaunch turtlebot3_gazebo turtlebot3_world.launch

先ほどと同じGazeboが立ち上がります．続いて，地図を作成するプログラムを実行します．

roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping

下記コマンドでロボットが自律走行を行いますので，地図をある程度完成させます．

roslaunch turtlebot3_gazebo turtlebot3_simulation.launch

地図が完成したら，map serverというソフトをインストールします．

保存したいフォルダ(ここではmaps)を作成し，そこに保存します．

mkdir ~/maps
cd ~/maps
rosrun map_server map_saver

一度今開いているプログラムを一度閉じてGazeboを起動しなおします．

roslaunch turtlebot3_gazebo turtlebot3_world.launch

下記コマンドを実行します．

roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/maps/map.yaml

すると，以下の図のように表示されます．この図には、読み込んだ地図に基づく Global Map と、LiDAR に基づく Local Map の二種類が表示されている状態になっています．黒い点がGlobal mapで，ずれた位置に表示されているのがLocal Mapです．これらは通常一致すべきですが，ロボットの自己位置が正しく認識されていないのでずれた状態で表示されています．

手動で位置を合わせることが出来ます．2D Pose Estimateをクリックし，Gazebo上のロボットの位置あたりの場所をクリック＆ドラッグし，表示される矢印の向きをロボットの向きになるよう調整します．この後自動で一致しますので，大体で大丈夫です．

続いてゴールを決めて自律走行させてみます．2D New Goalをクリックし，地図上の任意の場所でクリック＆ドラッグをして，ゴールの場所とロボットの向きを指定します．
すると，自動でロボットが走行します．また走行するにつれ，自己位置が補正され，マップの不完全だった場所も自動で再構成されます．

3.ROSの可視化

rqt_graphを使ってノードの可視化をしてみます。

rqt_graph

rqt_graphの表示を見ると、move_baseが、gazebo上で、robot_state_publisherからロボットの情報を取得し、map_serverから地図データを取得し、amclで自己位置を推定しながらナビゲーションを行っていることが分かります．

ではこの状態でロボットの進行速度を可視化してみます．Rqt_plotを使うと現在PublishされているTopicの値をグラフにすることが出来ます．センサーの値を確認しながら動かしたいときに非常に便利です．

rqt_plot

rqt_plotでは現在PublishされているTopic名とその中の変数をつないで書くことでグラフにプロットしたい要素を指定します．現在動いているシステムのcmd_velのTopicのlinear.xを表示したい場合は左上のTopicのテキストボックスに/cmd_vel/linear/xと記入し+ボタンを押すか，rqt_plotの起動時に以下のコマンドのように指定する事で，グラフとして可視化できます．設定した後，新たなGoalを指定し，ロボットを動かしてみてください．グラフがリアルタイムで更新されます．

rqt_plot /cmd_vel/linear/x

表示だけでなく，TopicをPublishするGUIもあります．Gazeboだけ残し，他のプログラムは終了させ，下記のコマンドでプログラムをインストールしてください．

sudo apt-get install ros-noetic-rqt-ez-publisher

下記コマンドで実行されます．

rosrun rqt_ez_publisher rqt_ez_publisher

するとコンボBOX内で現在Masterに登録されているTopicを選択できます．今回はロボットを操作してみたいので，/cmd_velを選択してみてください．無ければ左上の更新ボタンを押します．スライダーで動かすたびにロボットが移動したと思います．実際にどの値をPublishすれば目的の動きをするかすぐに試すことが出来るので便利です．

4.オリジナルGUIの作成

このようなモジュールを組み合わせ自身でGUIを作ることも可能です．下記コマンドで空のrqtを起動します．

rqt

起動した状態では何も表示されていませんが，メニューバーのPlugins⇒Visualization⇒Plotを選択してみましょう．

次に，Plugins⇒Topics⇒Easy Message Publisherを選択します．先ほどのrqt_ez_publisherの様なスライダーを追加できました．このように自分の好きなGUIを簡単に作る事が出来ます．色々操作して下記のようにcmd_vel/linear.xとangular.zを操作しながらグラフで表示できるGUIを作ってみてください．

おわりに

今回はRvizとrqtを使って，センサー情報の可視化を行ってみました．
センサーの可視化の対象がシミュレータ上のロボットでしたが，
もちろん実際のロボットのトピックも参照する事が出来るので，
デバッグが非常に楽になります．是非使いこなしてみてください．