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パッケージのインストールと設定変更を行う。
$ 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カメラの動作確認
$ roscore
$ roslaunch video_stream_opencv webcam.launch
$ 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ファイルを調整する。
$ cd ~/catkin_wsgc/src/cartographer_ros/cartographer_ros/configuration_files
$ nano 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ファイルへの記録
ロボットに部屋中を走り回らせて地図を生成する。
床に置かれた障害物が多いため、真空地帯が多数出現する想定。
$ roscore
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200
$ sudo rfcomm listen /dev/rfcomm0 1
Windows10PC側で TeraTerm を起動し、RaspberryPiへRFCOMM接続する。
$ rosrun zumo32u4 zumo32u4.py
$ cd ~/catkin_wsgc
$ source install_isolated/setup.bash
$ roslaunch cartographer_ros backpack_2d.launch
$ roslaunch video_stream_opencv webcam.launch
$ export ROS_MASTER_URI=http://raspberrypi.local:11311/
$ rosbag record /camera/camera_info /camera/image_raw command imu odom scan sensorval
$ 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 の内容**
<?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 の内容**
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 の内容**
<?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による自宅の地図作成 【成功！！】 へ続く