zumo32u4　
#◆ 前回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_013日目_SLAM_VINS-Mono　の続き

#◆ はじめに
今回はいよいよ自宅一室の地図をSLAMで作成してみる。
足回りが段差を超えることに適していないため、あくまで一室のみに限定する。
どうしても段差を超えたくなったときには足回りを取り替えることにする。
アルゴリズムは、前回までの検証で精度が高かった Google Cartographer を採用する。

今回は真面目に、

Bluetooth経由でロボットを遠隔操作
RaspberryPi ＋ Arduino ＋ RPLidar A1M8 を連携させてSLAM / 地図生成
Wi-Fi経由でUbuntuPC側でリアルタイムに地図を可視化
地図のチューニング

に、チャレンジしようと思う。
地図生成は自律航行をさせるうえでの重要な要素、ということなのでしっかり取り組みたい。

##● 地図生成環境の概要図
　

##● 環境

Ubuntu 16.04 (rvizによる地図可視化用PC)
Windows10 + TeraTerm (ロボット操作用PC)
Zumo32u4 (ロボット本体)
RaspberryPi3 + Raspbian Stretch (ロボット本体、CartoGrapher)
RPLidar A1M8 (ロボット本体、測域)
ELP-USB8MP02G-L75 (USBカメラ、動画撮影用)

##● ロボットの再構成と外観
RPLidar A1M8 と USBカメラをそれぞれRaspberryPiのUSBポートへ接続し、下図のとおり固定。

案の定、ついに重戦車と化す。
設計のノウハウもへったくれもない素人が作るとこうなる、の図。
はじめに へ掲載した外観画像から更にスゴみを増している。タワーかよ。
後からくっつけたUSBカメラが砲塔のように見えるが、戦車を作る趣味は無い。
引いて撮影した３枚目の画像を見るとサイズ感が伝わるが、やたらと大きいわけでもない。
縦横：１０ｃｍ四方
高さ：２０ｃｍ弱

流石にこの時点で無理がタタった。
5Vモバイルバッテリー駆動の状態でLiDARをRaspberryPiへ接続すると、電源電圧が不足してRaspberryPiが起動しない。電圧不足、久々だ。。。
ちなみに Physical Computing Lab TSI-PI036-5V3A から給電すると余裕で正常起動する。
RPLidarA1M8の データシート を額面どおりに読むと、LiDAR側へは500mAが定常的に供給されないとまともに駆動しない。ように見える。（実際は6.6Hz駆動で260mAほどしか消費しない）
現時点で、

モーター駆動用
RaspberryPi本体稼働用メイン
RaspberryPi本体稼働用サブ

の合計３つのバッテリを搭載しているが、更にLiDAR専用のバッテリを搭載し、追加したバッテリの充電機構をあらためて練り直す必要がありそうだ。
取り急ぎ、逆流防止のダイオード及び過電流を防止する保護素子を搭載した設計のUSBケーブルを調達。

低電圧になるRaspberryPi側へ電流が逆流しないようにし、ひとまず追加部材の組み合わせだけで電圧不足は回避できたようだ。
なお、追加バッテリ側の電圧が降下したときにRaspberryPiがダウンしてしまう可能性は依然として残るため、追加バッテリ側の充電機構(充電タイミングの制御)は追って考えるものとする。
エレコム Y字Wパワーケーブル USB2.0 Aメス簡易パッケージ USB-AAE5DPBK

これでも、追加したバッテリ側には1000mAほど出力に余裕があるようなので、どうせなら電源を分岐して、ゆくゆくは Intel Movidius Neural Compute Stick もブッ挿してしまおうと思う。

#◆ 参考にさせていただいた資料、謝辞
【Koji Teradaさん】
TF,TF2 完全理解 SlideShare

【nnn112358さん】
ROSのTFへOdometoryを入力する Qiita

#◆ 作業に掛かる前の前提条件
Raspbian Stretch+NCS(Neural Compute Stick)+YoloV2+Webカメラ＋ROSによるリアルタイム複数動体検知環境の構築 と ００２日目００３日目００４日目００５日目００６日目００８日目００９日目 の記事を参考に、全ての作業を実施済みであり、導入済みパッケージの種類と数の同期がとれていることを前提とする。

#◆ 動画撮影用の環境設定
##● USBカメラ使用環境のキッティング
ロボット本体の動作には全く関係しないが、ロボットを走行させながらUSBカメラで動画撮影も行いたいため、RaspberryPiへUSBカメラ連携用ROSパッケージのインストールと設定変更を行う。

RaspberryPiへUSBカメラ連携用ROSパッケージ（OpenCVベース）のインストール

$ cd ~/catkin_ws/src
$ git clone https://github.com/PINTO0309/video_stream_opencv.git
$ cd ..
$ catkin_make
$ nano src/video_stream_opencv/launch/webcam.launch

下記のとおり編集する。

編集前

<arg name="camera_name" value="webcam" />
<arg name="set_camera_fps" value="30"/>
<arg name="fps" value="30" />
<arg name="visualize" value="true" />
<!--
<arg name="width" value="0"/>
<arg name="height" value="0"/>
-->

編集後

<arg name="camera_name" value="camera" />
<arg name="set_camera_fps" value="2"/>
<arg name="fps" value="2" />
<arg name="visualize" value="false" />
<arg name="width" value="320"/>
<arg name="height" value="240"/>

##● USBカメラの動作確認

RaspberryPiで実行

$ roscore

RaspberryPiで実行

$ roslaunch video_stream_opencv webcam.launch

UbuntuPCで実行

$ export ROS_MASTER_URI=http://raspberrypi.local:11311/
$ rosrun rviz rviz

UbuntuPC側で起動してきた rviz画面左下の Add ボタンをクリック。

By topic タブを選択して Image を選択し、OKボタンをクリック。

RaspberryPiに接続されたUSBカメラから、Ubuntu側で起動中のrvizへ正常に映像をPublishできたようだ。

#◆ rvizファイルの修正
バグ対応のため、rvizファイルを調整する。

UbuntuPC側でrvizファイルを修正

$ cd ~/catkin_wsgc/src/cartographer_ros/cartographer_ros/configuration_files
$ nano demo_2d.rviz

下記のとおりファイルを修正する。

**demo_2d.rviz の内容**

demo_2d.rviz

Panels:
  - Class: rviz/Displays
    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Submaps1
        - /PointCloud21
        - /Map1
      Splitter Ratio: 0.600670993
    Tree Height: 322
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.588679016
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: PointCloud2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.0299999993
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 100
      Reference Frame: <Fixed Frame>
      Value: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
        base_link:
          Value: true
        horizontal_laser_link:
          Value: true
        map:
          Value: true
        odom:
          Value: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        map:
          base_link:
            horizontal_laser_link:
              {}
            odom:
              {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        map:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        odom:
          Alpha: 1
          Show Axes: false
          Show Trail: false
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Class: Submaps
      Enabled: true
      Fade-out distance: 1
      High Resolution: true
      Low Resolution: false
      Name: Submaps
      Submap query service: /submap_query
      Submaps:
        All: true
        Trajectory 0:
          0.43: true
          Value: true
      Topic: /submap_list
      Tracking frame: base_link
      Unreliable: false
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/PointCloud2
      Color: 0; 255; 0
      Color Transformer: FlatColor
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 4096
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: PointCloud2
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.0500000007
      Style: Flat Squares
      Topic: /scan_matched_points2
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /trajectory_node_list
      Name: Trajectories
      Namespaces:
        Trajectory 0: true
      Queue Size: 100
      Value: true
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /landmark_poses_list
      Name: Landmark Poses
      Namespaces:
        {}
      Queue Size: 100
      Value: true
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /constraint_list
      Name: Constraints
      Namespaces:
        Inter constraints, different trajectories: true
        Inter constraints, same trajectory: true
        Inter residuals, different trajectories: true
        Inter residuals, same trajectory: true
        Intra constraints: true
        Intra residuals: true
      Queue Size: 100
      Value: true
    - Alpha: 0.699999988
      Class: rviz/Map
      Color Scheme: map
      Draw Behind: false
      Enabled: true
      Name: Map
      Topic: /map
      Unreliable: false
      Use Timestamp: false
      Value: true
    - Class: rviz/Image
      Enabled: true
      Image Topic: /camera/image_raw
      Max Value: 1
      Median window: 5
      Min Value: 0
      Name: Image
      Normalize Range: true
      Queue Size: 2
      Transport Hint: raw
      Unreliable: false
      Value: true
  Enabled: true
  Global Options:
    Background Color: 100; 100; 100
    Default Light: true
    Fixed Frame: map
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Topic: /initialpose
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Angle: 0
      Class: rviz/TopDownOrtho
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.0599999987
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.00999999978
      Scale: 92.3554459
      Target Frame: <Fixed Frame>
      Value: TopDownOrtho (rviz)
      X: 0.837544441
      Y: 1.4433645
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 848
  Hide Left Dock: false
  Hide Right Dock: true
  Image:
    collapsed: false
  QMainWindow State: 000000ff00000000fd0000000400000000000001c5000002c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000183000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001b10000013d0000001600ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000434000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1535
  X: 55
  Y: 14

#◆ 地図生成の実行とbagファイルへの記録
ロボットに部屋中を走り回らせて地図を生成する。
床に置かれた障害物が多いため、真空地帯が多数出現する想定。

RaspberryPiで実行、Masterの起動

$ roscore

RaspberryPiで実行、rosserialの起動

$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

RaspberryPiで実行、RFCOMMを有効化

$ sudo rfcomm listen /dev/rfcomm0 1

Windows10PC側で TeraTerm を起動し、RaspberryPiへRFCOMM接続する。

RaspberryPiで実行、zumo32u4の起動

$ rosrun zumo32u4 zumo32u4.py

RaspberryPiで実行、GoogleCartoGrapherの起動

$ cd ~/catkin_wsgc
$ source install_isolated/setup.bash
$ roslaunch cartographer_ros backpack_2d.launch

RaspberryPiで実行、ビデオ撮影

$ roslaunch video_stream_opencv webcam.launch

UbuntuPCで実行、bagファイルへの記録

$ export ROS_MASTER_URI=http://raspberrypi.local:11311/
$ rosbag record /camera/camera_info /camera/image_raw command imu odom scan sensorval

UbuntuPCで実行、rvizの起動

$ cd ~/catkin_wsgc
$ catkin_make_isolated --install --use-ninja
$ source install_isolated/setup.bash
$ export ROS_MASTER_URI=http://raspberrypi.local:11311/
$ roslaunch cartographer_ros backpack_2d.launch

##● 試行１回目【失敗事例その１】
電源類を全てバッテリのみでまかなうことに成功したため、ワクワクしながら遠隔操作で初めて単独航行させてみた様子。

結果、見事に失敗。
機体の前進と回転に対してマップが追随していない。
LiDARで検出した点群だけがリアルタイムに反応してしまっている。
機体のオドメトリあるいは IMU を正常に拾えていないのか、はたまた別の理由なのか。
まだ、実装面にバグがありそう。
少し真面目に調べる必要がありそうだ。

##● 試行２回目【失敗事例その２】
rvizの設定におかしなところが複数あったため、内容を見直す。
◆ rvizファイルの修正 に記載の対応を行った。
ウォッチすべきTOPIC名の記載を数カ所間違えていた。
また併せて、Arduinoへ書き込んでいたスケッチにオドメトリが正常にPublishされないバグがあったため修正。
さて、どうかな。

またしても失敗。
今度は機体が動く。動くんだけど・・・高速に動きすぎて一瞬で遥か彼方へ遠ざかってしまう。
もう少し控えめに動いてくれてもいいんだけど。。。
どう見ても、機体と地図のスケールが合っていないように見える。
エンコーダ値の変換の仕方がマズいのだろうか。
また、直進しているはずなのにわずかに斜めに進んでいることになっている。
左右の荷重が均等ではないため、左右のモーターに掛かる負荷が同じにはならず、平等なパワーを与えてしまうと左右の車輪の前進距離に少しずつズレが生じることが分かった。

##● 試行３回目【失敗事例その３】
TF周りのリンクがおかしいのかもしれない、ということで backpack_2d.launch、backpack_2d.lua ファイルの修正とリビルドを実施。

**RaspberryPi側の backpack_2d.launch の内容**

backpack_2d.launch

<?xml version="1.0" ?>
<launch>
  <param
      name="robot_description"
      textfile="$(find zumo32u4)/urdf/zumo32u4.urdf"
  />

  <node
     name="horizontal_laser"
     pkg="rplidar_ros"
     type="rplidarNode"
     output="screen">
     <param name="serial_port"      type="string" value="/dev/ttyUSB0"/>
     <param name="serial_baudrate"  type="int"    value="115200"/>
     <param name="frame_id"         type="string" value="horizontal_laser_link"/>
     <param name="inverted"         type="bool"   value="false"/>
     <param name="angle_compensate" type="bool"   value="true"/>
  </node>

  <node
      pkg="tf"
      type="static_transform_publisher"
      name="base_link_connect"
      args="0 0 0 0 0 0 /base_link /horizontal_laser_link 100"
  />

  <node
      name="cartographer_node"
      pkg="cartographer_ros"
      type="cartographer_node"
      args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename backpack_2d.lua"
      output="screen">
  </node>

  <node
      name="cartographer_occupancy_grid_node"
      pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node"
      args="-resolution 0.05"
  />
</launch>

**RaspberryPi側の backpack_2d.lua の内容**

backpack_2d.lua

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

return options

URDFも無駄な記載が多数あったためシンプルに見直し。

**Ubuntu側の zumo32u4.urdf の内容**

zumo32u4.urdf

<?xml version="1.0"?>
<robot name="zumo32u4">
    <link name="imu">
    <link name="base_link">
        <visual>
            <origin rpy="0 0 3.14" xyz="0 0 0"/>
            <geometry>
                <mesh filename="package://zumo32u4/urdf/zumo32u4.STL" scale="1 1 1"/>
            </geometry>
            <material name="Grey">
                <color rgba="0.30 0.30 0.10 1.0"/>
            </material>
        </visual>
    </link>
    <joint name="jointimu" type="fixed">
        <parent link="base_link"/>
        <child link="imu"/>
        <origin rpy="0 0 0" xyz="0 0 0"/>
    </joint>
</robot>

が、状況変わらず。。。
真っ直ぐ動けないのはあとでキャリブレーションするのでともかくとして、依然として並進スピードが早すぎる。。。
現実世界では10cm程度しか移動していないのに10mほど移動したことになってしまう。

現時点のTFツリーは下図のとおり整頓。

#◆ 本日のまとめ

Github を随時更新中。
機体は必要な素材がほぼすべて組み込めた。あとは色素増感太陽電池と Neural Compute Stick を挿すだけ。
電力不足にみまわれるなどの物理的な問題の解消に数日を浪費し、ここまで到達するのに丸一週間かかってしまった、が、未だに成功していない。
あと少しなのに原因をつかむのが雲をつかむように難しい。
「ループクロージャーを有効にすると異常に加速する。」という Issue を見た気がするが、英文の解釈に自信が無い。
良心的な方、同じような問題に遭遇された方、なにとぞ助けてやってください。。。調べるべきポイントなど、少しのヒントだけでも、ものすごく助かります。

#◆ 次回予告
CartoGrapherによる一部屋分の地図生成検証の継続あるいは一時的にGmappingへシフトして一部屋分の地図生成検証、モーター制御のキャリブレーションを行おうと思う。

【物理的な誤差のダイナミックアジャスト Zumo 32U4 Synchronize Motor】
http://www.abrowndesign.com/2017/02/25/zumo-32u4-synchronize-motor/

#◆ 次回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_015日目_SLAM_自律走行の前準備_CartoGrapherによる自宅の地図作成【成功！！】　へ続く