# シミュレータ上のラズパイマウスを動かす方法 Part6

## ラズパイマウスを動かすまでの流れ

1. [距離センサの値の読み取り方](how_to_control_raspimouse_on_sim_1)
2. [モータの動かし方](how_to_control_raspimouse_on_sim_2)
3. [キーボードを用いたラズパイマウスの動かし方](how_to_control_raspimouse_on_sim_3)
4. [コントローラを用いたラズパイマウスの動かし方](how_to_control_raspimouse_on_sim_4)
5. [距離センサの値を利用したモータの動かし方](https://github.com/yukixx6/raspimouse_sim_tutorial/tree/4c5b11f5718b3dfe06667b9146522232fb35bc13/tutorial/how_to_control_raspimouse_sim_5.md)
6. [測域センサ(URG)を用いたSLAMの行い方](https://github.com/yukixx6/raspimouse_sim_tutorial/tree/4c5b11f5718b3dfe06667b9146522232fb35bc13/tutorial/how_to_control_raspimouse_sim_6.md) ←今ここ

Part6では測域センサ(URG)を用いてSLAMを行っていきましょう。

URGとは、北陽電気株式会社さんから発売されている測域センサで、前方240°を検出できます。

公式サイト : [URG-04LX-UG01](https://www.hokuyo-aut.co.jp/search/single.php?serial=17)

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr0_qMRffYWV54vis7%2F-LTr0aUNIYBQtoC2xhQr%2Furg.jpg?generation=1544963912465154\&alt=media)

## SLAMについて

SLAMとは、Simultaneous Localization and Mappingの略で、自己位置推定と地図生成を同時に行うものです。

SLAMにはLIDER(Light Detection and Ranging)呼ばれるセンサを使用します。 上記のURGもLIDERの一種です。

SLAMを行うときは**tf**と**scan**という2つのデータが必要になります。

### 大まかな流れ

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LXJTAo6m575Ad6KwUn3%2F-LTr0aUPZbuiDCViiMnv%2Fslam.png?generation=1548679169974055\&alt=media)

### tf

ロボットを動かす上で必要になる、ロボットの位置姿勢や座標を変換したり配信しているトピックです。 ロボットの移動量(odometry, odom)から座標変換したパラメータを使用します。 `tf`トピックで配信しています。

### scan

URGから取得できるセンサ値を含まれているトピックです。`scan`トピックで配信しています。

### SLAM

自己位置推定と地図生成を同時に行っています。 地図生成に関しては`tf`トピックと`scan`トピックから、地図を生成するための`map`データを生成しています。

### map\_server

SLAMによって出来た`map`データから地図画像(`.pgm`)と地図データ(`.yaml`)を生成しています。 他に地図画像(`.pgm`)と地図データ(`.yaml`)より地図情報を`map`トピックで配信することが出来ます。

## 前準備

まずURGを動かすためのROSパッケージをインストールします。 すでに`urg_node`をインストールしている場合は行わなくて大丈夫です。

```
sudo apt install ros-kinetic-urg-node
```

次に、SLAMのROSパッケージをインストールします。

```
sudo apt install ros-kinetic-slam-gmapping
```

最後に`map_server`をインストールしましょう。

```
sudo apt install ros-kinetic-map-server
```

以上で準備完了です。

## トピックの確認

URGを使用するには先程インストールした`urg_node`が必要になりますが、Gazeboを起動すると同時に起動します。

**URG付きラズパイマウスは以下のコマンドで起動します。**

```
roslaunch raspimouse_gazebo raspimouse_with_willowgarage.launch
```

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr0_qMRffYWV54vis7%2F-LTr0aUXQVkAO29LOkiq%2Fraspimouse_sim_willowgarage.png?generation=1544963912261106\&alt=media)

tfの相対座標変換の見てみましょう。

```
roslaunch raspimouse_ros_2 raspimouse.launch
rosrun rqt_tf_tree rqt_tf_tree
```

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr0_qMRffYWV54vis7%2F-LTr0aUZPmeJqmWg6Gjz%2Ftf_tree.png?generation=1544963912949391\&alt=media)

`odom`から座標変換されて`base_link`になり、さらに座標変換して枝分かれしてることがわかります。 これらの情報を`tf`トピックは配信しています。

次にURGから配信されるセンサ値を確認してみましょう。

センサ値は`/scan`トピックで配信されます。

```
rostopic list
```

と打ち、`/scan`トピックがあることを確認し、

```
rostopic echo /scan
```

と打つと、数字がたくさん表示されると思います。

上に遡って見てみると以下のようになっています。

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr0_qMRffYWV54vis7%2F-LTr0aUaKDzPSMApvPFQ%2Fscan_topic.png?generation=1544963912260156\&alt=media)

画像の **ranges: \[inf,inf, ... ]**&#x306E;部分がセンサ値になります。カンマで区切られている値1つ1つがセンサ値で、URGが検出できる前方240°分あります。

センサ値が大きいほどURGと壁や物との距離が遠く、値が小さいほど距離が近いです。また遠すぎすると**inf**(infinity)が出ます。

### センサ値の可視化

センサ値の可視化にはRvizというGUIツールを使用します。 Rvizの起動には以下のコマンドを使用します。

```
rviz rviz
```

起動したら、**Fixed Frame**の**urg\_lrf\_link**に変更します。

次に画面左下の**Add**を押し**By display type**の**LaserScan**を追加し、**Topic**&#x3092;**/scan**にします。

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr0_qMRffYWV54vis7%2F-LTr0aUcK_tlpMlKqjTK%2Frviz1.png?generation=1544963912208702\&alt=media)

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr0_qMRffYWV54vis7%2F-LTr0aUeVyM62xdDyRRg%2Frviz2.png?generation=1544963913830631\&alt=media)

起動時のシミュレータは以下の図のようになっています。 上のRvizの図と見比べると、赤い点群がシミュレータの壁と対応していることがわかります。

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr0_qMRffYWV54vis7%2F-LTr0aUgh2kF6DWWb6Qq%2Fsim_urg.png?generation=1544963913853588\&alt=media)

## gmappingのパラメータを設定

gmappingはSLAMのアルゴリズムの1つで、正確に地図作成するために様々なパラメータを設定する必要があります。パラメータはlaunchファイルに記述します。

今回は、`raspimouse_sim_gmapping.launch`という名前にします。

```
<launch>
 <include file="$(find raspimouse_game_controller)/launch/run_with_base_nodes.launch" />

<!-- <node pkg="urg_node" name="urg_node" type="urg_node" required="true" > -->
<!--  <param name="frame_id" value="base_link" /> -->
<!-- </node> -->

 <node pkg="rviz" type="rviz" name="rviz" args="-d $(find raspimouse_sim_tutorial_program)/config/gmapping.rviz" /> 

 <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
  <param name="base_frame" value="base_link" />
  <param name="odom_frame" value="odom" />

  <param name="maxUrange" value="4.0" />
  <param name="maxRange" value="4.0" />

  <param name="xmin" value="-15" />
  <param name="ymin" value="-15" />
  <param name="xmax" value="15" />
  <param name="ymax" value="15" />

  <param name="srr" value="0.1" />
  <param name="srt" value="0.1" />
  <param name="str" value="0.1" />
  <param name="stt" value="0.1" />

  <param name="particles" value="30" />
 </node>
</launch>
```

### コード解説

```
 <include file="$(find raspimouse_game_controller)/launch/run_with_base_nodes.launch" />
```

チュートリアルの[コントローラを用いたラズパイマウスの動かし方](how_to_control_raspimouse_on_sim_4)で使用した、ロボットをコントローラで動かすためのlaunchファイルを起動しています。

```
<!-- <node pkg="urg_node" name="urg_node" type="urg_node" required="true" > -->
<!--  <param name="frame_id" value="base_link" /> -->
<!-- </node> -->
```

`urg_node` を起動するための記述ですが、 `urg_node`はGazeboの起動と同時に立ち上がるため、コメントアウトしています。

```
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find raspimouse_sim_tutorial_program)/config/gmapping.rviz" />
```

Rvizを起動します。 今回はgmapping用のRvizの設定([gmapping.rviz](https://github.com/yukixx6/raspimouse_sim_tutorial_book_sample_program/blob/add_slam/config/gmapping.rviz))を作っておいたので、こちらを使用します。

```
 <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
  <param name="base_frame" value="base_link" />
  <param name="odom_frame" value="odom" />

  <param name="maxUrange" value="4.0" />

  <param name="xmin" value="-15" />
  <param name="ymin" value="-15" />
  <param name="xmax" value="15" />
  <param name="ymax" value="15" />

  <param name="srr" value="0.1" />
  <param name="srt" value="0.1" />
  <param name="str" value="0.1" />
  <param name="stt" value="0.1" />

  <param name="particles" value="30" />
 </node>
```

ここでgmappingのパラメータの設定をしています。

`base_frame`はロボットのメインフレームで`base_link`、 `odom_frame`はオドメトリのフレームで`odom`を設定しています。

`maxUrange`でURGの最大測定範囲を設定しています。

`xmin`と`ymin`、`xmax`、`ymax`で地図の大きさを設定しています。

以下の4つはそれぞれオドメトリの誤差の設定をしています。この値が大きすぎても小さすぎても正確な地図は出来ません。今回はすべて0.1にしました。

`srr`：移動したときの移動の誤差

`srt`：移動したときの回転の誤差

`str`：回転したときの移動の誤差

`stt`：回転したときの回転の誤差

`particles`はパーティクルの数を設定しています。この値が大きいほど正確な地図ができやすいですが、計算量が増えてしまいます。今回はデフォルトの30にしました。

## SLAMを行う

まず以下の2つを起動させます。

```
roslaunch raspimouse_gazebo raspimouse_with_willowgarage.launch
```

```
roslaunch raspimouse_sim_tutorial_book_sample_program raspimouse_sim_gmapping.launch
```

起動したら、コントローラを使用してラズパイマウスを動かして地図を作っていきます。 初期位置にいる部屋を壁に沿って一周してみましょう。

一周させたら地図を保存しましょう。下のコマンドではroomという名前で保存しています。

```
roscd raspimouse_sim_tutorial_program/
rosrun map_server map_saver -f map/room
```

## 実行結果

![](https://164413620-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LQj-2xtXRtxfWAIKc7B%2F-LTr2XGDOjk0OR82L8lz%2F-LTr2Y22iVhOSfzlCMrK%2Fsample_map.png?generation=1544964422404338\&alt=media)

### 行っている様子

YouTubeで公開しています。

&#x20;[\[Raspberry Pi Mouse Simulator\] チュートリアルPart6 測域センサ(URG)を用いたSLAMの行い方](https://www.youtube.com/watch?v=PUY2l-049CE)
