ROSの勉強　第22弾：ナビゲーション

# #プログラミング ROS< ナビゲーション >

## はじめに
1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第22弾として，「ナビゲーション」を扱う．


## 環境
##### 仮想環境
|||
|:-:|:--|
|ソフト|VMware Workstation 15|
|実装RAM|2 GB|
|OS|Ubuntu 64 ビット|
|isoファイル|ubuntu-mate-20.04.1-desktop-amd64.iso|

##### コンピュータ
|||
|:-:|:--|
|デバイス|MSI|
|プロセッサ|Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz|
|実装RAM|8.00 GB (7.89 GB 使用可能)|
|OS|Windows (Windows 10 Home, バージョン：20H2)|

##### ROS
|||
|:-:|:--|
|Distribution|noetic|
|プログラミング言語|Python 3.8.5|
|シミュレーション|gazebo|

## 世界を動き回る
ロボットができる最も基本的なことの1つは世界を動き回ることである．以下にまとめを示す．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/b58e8c36-fe19-1c93-d3fc-1ffb1677f4c7.png)
地図は前回

https://qiita.com/Yuya-Shimizu/items/fbc2a9c6c7810f6af5e6

までに学習済みであるから，ここでは，上で示したとおり，「出発地」と「目的地」について扱う．

## 位置の推定
まず，ロボットがどこにいるかを見つけ出す方法を学ぶ．以下に位置推定についてまとめた．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/f2ef6efe-72e1-7028-6106-e34a68dfe14b.png)

## 実装
ここからは，実際にシミュレーション上で動かしながら，実装方法を学ぶとともに，ナビゲーションについての理解を深める．以下にその手順とそのときの実行様子を示す．

**必要なノードを起動**
![キャプチャ.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/c86f0489-1d98-37a3-eba9-7e73d120b69c.png)
それぞれのターミナル(端末)での実行内容を以下に示す．

```:ターミナル(左上)；gazebo起動
roslaunch turtlebot3_gazebo turtlebot3_world.launch
```
```:ターミナル(真上)；地図サーバ起動
rosrun map_server map_server map2.yaml
```
```:ターミナル(左下)；位置推定のためのamcl起動
roslaunch turtlebot3_navigation amcl.launch
```
**※初回では，次の2つをしていなければエラーが出てしまう．
``sudo apt-get install ros-noetic-amcl``
``sudo apt-get install ros-noetic-dwa-local-planner``** $^{(2)}$

```:ターミナル(真下)；ナビゲーション移動のためのmove_base起動
roslaunch turtlebot3_navigation move_base.launch
```
**※初回では，次の操作をしていなければエラーが出てしまう．
``sudo apt-get install ros-noetic-move-base``**

```:ターミナル(右端)；可視化のためのrviz起動
rosrun rviz rviz
```

ここからは，rvizでの様子を示す．
**rvizの起動**
![_1.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/3f566f61-a8db-2c82-b494-d3336016a60f.png)
**mapの反映準備**
![_2.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/7c09b5f7-737a-df67-807c-c556d2d3c2d0.png)
**mapのトピックを購読**
![_3.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/59c343f2-9340-bfbb-b757-5d370303d78a.png)
**map反映**
![_4.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/fee8e46e-30cb-f21a-5134-eb11ebbc607e.png)
**ロボットモデルの反映準備**
![_5.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/111e4ef1-3ead-e206-5f92-9690c0a9c765.png)
**ロボットモデルの反映**
![_6.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/64f3b5b1-d61f-f8ca-0e91-ed31d8fd1867.png)
**候補となる姿勢を矢印で示すparticle arrowの反映準備**
![_7.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/eb163ec1-182b-f13e-fab2-684c3c609391.png)
**particle arrowのトピックを購読**
![_8.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/69c0326b-00df-72e8-1249-8d3f12be8bae.png)
以下に，動画像でそのparticle arrowの様子を示す．
![ezgif-2-6dd81d70de96.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/602d8cb7-4e83-6a1a-d39c-7f68f8ec534d.gif)
ロボットを好きな位置にできる．初期姿勢を自由に指定できるということである．ただし，ロボットの初期位置が本当に分からない時にのみ適当に指定するのが好ましい．
![t.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/a21d442e-52ff-bb39-0317-67e8b21b1317.gif)
たくさん動かすことで，センサの情報などから候補が収束していく様子がうかがえる．なお，ここでは新たなターミナル(端末)で``roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch``を実行してロボットの操作をしている．


続いては，rviz上でのナビゲーションについての様子を示す．
**現在の目標値を可視化するものを準備**
![_9.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/4a27b36c-016c-bfe7-1d3f-5933d0602774.png)
**グローバル経路を示すものを準備**
![_10.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/43511d2d-c781-f056-d051-7911aabdaa9f.png)
**ローカル経路を示すものを準備**
![_11.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/07ad1a6c-ffc7-df98-8b0e-d41a0c6433a6.png)
ここで，グローバル経路とローカル経路について簡単に触れておく．グローバル経路は現在の位置から目標までロボットがたどろうとするもの（計画）であるのに対して，ローカル経路は大局的な経路をたどることと地図上にはないがロボットのセンサが検出した障害物を避けることとのバランスをとる（実際に移動する経路）．
**rviz上でのナビゲーション**
アップロード容量の関係により，3つに分割して実行の様子を示している．
![1](https://user-images.githubusercontent.com/61382602/110401200-7d571c80-80bc-11eb-971c-f48bb1800985.gif)
![2](https://user-images.githubusercontent.com/61382602/110401180-73351e00-80bc-11eb-9088-275914346f4c.gif)
![3](https://user-images.githubusercontent.com/61382602/110401186-74fee180-80bc-11eb-97ad-f0bcb0b54ad4.gif)


## costmap
ほかにもさまざまなツール（トピック）がある．ここでは，``global_costmap``や``local_costmap``だけを示しておく．これらを指定すると進むべき場所やどれくらいよいのか悪いのかということを各セルを明暗により表現したものを観察できる．
まず，その指定方法を示す．
**global_costmapのMAPを追加**
![_12.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/fd6d37f2-e96b-b05b-ff9d-3b9942ab3be8.png)
**local_costmapのMAPを追加**
![_13.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/70346026-ca9a-acc6-4bf1-b87ec32dc49c.png)

以下にその実行様子を示す．
##### global_costmap
![global_costmap](https://user-images.githubusercontent.com/61382602/110405579-25bcaf00-80c4-11eb-93ff-3c4c956e35a3.gif)
##### local_costmap
![local_costmap](https://user-images.githubusercontent.com/61382602/110405595-2a816300-80c4-11eb-9c8d-69415f9455de.gif)
##### global_costmapとlocal_costmap
![global_local_costmap](https://user-images.githubusercontent.com/61382602/110405748-64526980-80c4-11eb-9ba5-f3557ff704aa.gif)

グローバル経路とローカル経路と同様に，ローカルのコストマップの方がロボット周辺でセンサによる周囲状況の変更が多くみられることが分かる．

## コードによるナビゲーション
rvizで手作業でゴールを示すのではなく，もうルートがすでに決まっているようなもの（たとえばパトロール）は，あらかじめコードにまとめて実行することでナビゲーションを実現することができる．
以下にソースコードを示す．

#ソースコード
```python:patrol.py
#!/usr/bin/env python3

import rospy
import actionlib

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

#ロボットが巡回する経由地のリスト：位置と回転(四元数)
waypoints = [
    [(2.1, 2.2, 0.0), (0.0, 0.0, 0.0, 1.0)],
    [(6.5, 4.43, 0.0), (0.0, 0.0, -0.984047240305, 0.177907360295)]
]


def goal_pose(pose):  #waypointsをMoveBaseGoalメッセージにするヘルパー関数
    goal_pose = MoveBaseGoal()
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]

    return goal_pose


if __name__ == '__main__':
    rospy.init_node('patrol')

    #アクションクライアントを作成し，サーバの準備完了を待つ
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    
    #waypointsを繰り返し処理し，それぞれをアクションのゴールとする
    while True:
        for pose in waypoints:
            goal = goal_pose(pose)
            client.send_goal(goal)
            client.wait_for_result()
```
これは，教材（参考文献）のサンプルである．少し説明を加えているが，そもそも適用しているシミュレーション環境が今回とは異なるため巡回する経由地などは，改めて書き換える必要はある．しかしながら，パトロールのようなものは，このようにしてコードを書くことで実現できるということを学ぶことが本質であるため，今回は書きかえまではしていない．今後できたらと思う．簡単にまとめておく．rvizでも見たようにゴールを与えてあげることで，アクションが作動しそのゴールへ向かう．なので，目標値を逐次変更することで巡回するような動作を実現することができる．

## 感想
rvizでの操作が少しずつではあるが，慣れてきた．今回も様々なツールを学んだことで，できることも増えた．ただのマップ生成だけではなく，そのマップを活用したナビゲーションができるようになったことはロボット工学を学ぶ上で大きな一歩となったのではないかと感じている．しかしながら，まだまだ学ぶべきことがあると思うため，気を抜いてはならない．むしろ楽しみである．

## 参考文献
(1) プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　  　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行

(2) ROS kineticでmove base使用時にDWAPlannerROS planner周りのエラーが出た時の対策メモ
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　@seigot　2020年02月10日
　　　　　　　　　(https://qiita.com/seigot/items/a13558e92c84f676dd0a ;‎2021‎年‎3‎月‎8‎日参照)