SLAMして自己位置推定して自律移動する

#URGでセンサデータを取得して可視化

WHILL Auto Project(仮称)の第2弾です．
今回し要した外部パッケージやlaunchファイルは以下から取得できます．
https://github.com/nobunoby/WHILL_Auto

[前回の記事](https://qiita.com/noby_aoao/private/026068c944067d4a4288)

###package installation
参考: https://blog.browniealice.net/post/how_to_use_urg/

###urg scanをrvizにプロットする
まずurgのシリアルポートを取得する。

```shell
$ dmesg | grep tty
ubuntu@ubuntu:~/catkin_ws$ dmesg | grep tty
[    0.000000] Kernel command line: coherent_pool=1M 8250.nr_uarts=1 snd_bcm2835.enable_compat_alsa=0 snd_bcm2835.enable_hdmi=1 bcm2708_fb.fbwidth=0 bcm2708_fb.fbheight=0 bcm2708_fb.fbswap=1 smsc95xx.macaddr=DC:A6:32:84:C3:C1 vc_mem.mem_base=0x3ec00000 vc_mem.mem_size=0x40000000  net.ifnames=0 dwc_otg.lpm_enable=0 console=ttyS0,115200 console=tty1 root=LABEL=writable rootfstype=ext4 elevator=deadline rootwait fixrtc
[    0.000630] printk: console [tty1] enabled
[    2.860450] fe201000.serial: ttyAMA0 at MMIO 0xfe201000 (irq = 14, base_baud = 0) is a PL011 rev2
[    2.865215] printk: console [ttyS0] disabled
[    2.865257] fe215040.serial: ttyS0 at MMIO 0x0 (irq = 16, base_baud = 62500000) is a 16550
[    3.136632] printk: console [ttyS0] enabled
[   14.150074] cdc_acm 1-1.2:1.0: ttyACM0: USB ACM device
[   14.207231] usb 1-1.1: pl2303 converter now attached to ttyUSB0
[ 2534.856364] cdc_acm 1-1.2:1.0: ttyACM0: USB ACM device
[ 2590.927209] cdc_acm 1-1.2:1.0: ttyACM0: USB ACM device
[ 2665.406105] cdc_acm 1-1.2:1.0: ttyACM0: USB ACM device
[ 4263.617155] cdc_acm 1-1.2:1.0: ttyACM0: USB ACM device
[ 4305.274690] pl2303 ttyUSB0: pl2303 converter now disconnected from ttyUSB0
[ 4312.242110] cdc_acm 1-1.1:1.0: ttyACM0: USB ACM device
[ 4316.743410] usb 1-1.2: pl2303 converter now attached to ttyUSB0
```

ttyACM0とわかったので

```bash:raspi
rosrun urg_node urg_node _serial_port:="/dev/ttyACM0"
```

```shell:raspi
rosrun tf static_transform_publisher 0 0 0 0 0 0 world map 100 &
rosrun tf static_transform_publisher 0 0 1 -0.2 0 0 map laser 100 &
```

##SLAM

###Gmapping

参考：
[パラメータ解説](https://sy-base.com/myrobotics/ros/gmapping/)
[公式Wiki](http://wiki.ros.org/gmapping)

rvizでプロットが見える

```shell:pc
rosrun gmapping slam_gmapping
```

```shell:raspi
rosrun urg_node urg_node _serial_port:="/dev/ttyACM0" &
rosrun tf static_transform_publisher 0 0 1 -0.2 0 0 base_link scan 100 &
rosrun tf static_transform_publisher 0 0 0 0 0 0 world map 100 &
rosrun tf static_transform_publisher 0 0 0 3.1416 0 0 map odom 100 &
rosrun tf static_transform_publisher 0 0 1 0 0 0 base_link laser 100 &
```

生成された地図
![Screenshot from 2021-03-26 18-21-55.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/452637/38eca621-f26c-7548-b1f4-475fb943adb5.png)


##amclで自己位置推定
参考：
[ROSのナビゲーションamclについて理解を深めてみる](https://sy-base.com/myrobotics/ros/ros-amcl/)

###launchファイル作成

```whill_teleop.launch
<?xml version="1.0"?>
<launch>
  <arg name="make_map" default="false"/>

  <node name="joy" pkg="joy" type="joy_node" output="log">
  </node>
  <node name="joy_twist" pkg="teleop_twist_joy" type="teleop_node" output="log">
    <remap from="/cmd_vel" to="/whill/controller/cmd_vel" />
  </node>

  <node pkg="tf" type="static_transform_publisher" name="tf_base_scan" args="0 0 1 -0.2 0 0 base_link scan 100" />
  <node pkg="tf" type="static_transform_publisher" name="tf_map_odom" args="0 0 0 0 0 0 map odom 100" />
  <node pkg="tf" type="static_transform_publisher" name="tf_base_laser" args="0 0 1 0 0 0 base_link laser 100" />

  <group if="$(arg make_map)">
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="log">
      <param name="srr" value="0.01"/>
      <param name="srt" value="0.02"/>
      <param name="str" value="0.01"/>
      <param name="stt" value="0.02"/>
      <param name="linearUpdate" value="0.5"/>
      <param name="angularUpdate" value="0.436"/>
    </node>
  </group>


  <group unless="$(arg make_map)">
    <include file="$(find whill_launch)/launch/amcl.launch" >
    </include>
  </group>

  <group unless="$(arg make_map)">
    <node name="map_server" pkg="map_server" type="map_server" args="$(find whill_launch)/maps/srg_203.yaml"/>
  </group>

</launch>
```

```shell:pc
roslaunch whill_launch whill_teleop.launch 
```

```shell:raspi
rosrun urg_node urg_node _serial_port:="/dev/ttyACM0" &
roslaunch ros_whill ros_whill.launch
```


DS4で適当に動かした際の自己位置推定の様子
![Screenshot from 2021-03-26 20-08-16.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/452637/8f9af181-33b7-1aa7-20af-93e076dacb2e.png)



##move_baseで自律移動させる

参考
[move_baseの設定ファイルなど](http://wiki.ros.org/navigation/Tutorials/RobotSetup)
[navigation stackのパラチュン](http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
http://wiki.ros.org/move_base
http://wiki.ros.org/costmap_2d


```shell:raspi
rosrun urg_node urg_node _serial_port:="/dev/ttyACM0" &
roslaunch ros_whill ros_whill.launch &
```

```shell:pc
roslaunch whill_launch whill_auto.launch 
```


ノード、トピックはこんな感じ
![Screenshot from 2021-04-11 18-29-53.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/452637/c27d0f2d-4e95-457f-c50d-a8c83383368f.png)


###move_baseとteleopの優先順
move_baseでgoalを与えた後にDS4の□ボタンを押すとWHILLは一時停止した。指を離すと再度動き出した。
後出しが強いのか