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

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

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

前回の記事

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

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

$ 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とわかったので

raspi

rosrun urg_node urg_node _serial_port:="/dev/ttyACM0"

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

参考：
パラメータ解説
公式Wiki

rvizでプロットが見える

pc

rosrun gmapping slam_gmapping

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 &

生成された地図

##amclで自己位置推定
参考：
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>

pc

roslaunch whill_launch whill_teleop.launch 

raspi

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

DS4で適当に動かした際の自己位置推定の様子

##move_baseで自律移動させる

参考
move_baseの設定ファイルなど
navigation stackのパラチュン
http://wiki.ros.org/move_base
http://wiki.ros.org/costmap_2d

raspi

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

pc

roslaunch whill_launch whill_auto.launch 

ノード、トピックはこんな感じ

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