Python
C++
RaspberryPi
ROS
IoT

RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_015日目_SLAM_自律走行の前準備_CartoGrapherによる自宅の地図作成 【成功!!】

◆ 前回記事

RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_014日目_SLAM_自律走行の前準備_ロボット再構成と自宅の地図作成 【"沼" 突入中】 の続き

◆ はじめに

1週間に渡り Google CartoGrapher のパラメータ調整地獄の闇に触れ続け、ついぞ オドメトリ、つかわなくてもいいよ♪ という謎の天啓が舞い降りる。
ロボット Zumo32u4 が発行するオドメトリ? 何ソレ?

早速、 backpack_2d.luause_odometry パラメータを無効 「false」 にする、という暴挙に出る。

恒例どおり、先に結果のGIF画像から公開。(3倍速再生)
まさかのまさか、、、上手くいってしまいました!!
RaspberryPi で Google CartoGrapher による地図作成、できたぁ〜 ヽ(*゚∀゚)ノ ヤター
めちゃくちゃうれしい。 泣きそう。
ezgif.com-optimize (14).gif

● 地図生成環境の概要図

 0023_SWスタック図_RaspberryPi (7).png

● 環境

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

◆ 参考にさせていただいた資料、謝辞

実は、天啓の正体は下記に公開いただいている情報でした。
とても分り易く、目からうろこです。
おそらくこの情報がなければ、CartoGrapherは諦めていました。
かなり追い詰められていましたが、ここに来て徹夜祭りから開放されました。
最近、Qiitaを始めてからリスペクトすることがやたら多くなってきたなぁ。

【nnn112358さん】
「cartographer_ros ガイド」

◆ 作業に掛かる前の前提条件

Raspbian Stretch+NCS(Neural Compute Stick)+YoloV2+Webカメラ+ROSによるリアルタイム複数動体検知環境の構築002日目 003日目 004日目 005日目 006日目 008日目 009日目 014日目 の記事を参考に、全ての作業を実施済みであり、導入済みパッケージの種類と数の同期がとれていることを前提とする。

◆ backpack_2d.luaの修正

天啓に従い、オドメトリの使用を無効化する。
前回記事投稿時から CartoGrapherのパラメータ調整をアレコレ試し続けた結果、下記のように大きく記載内容を変更している。

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 = false,
  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.
TRAJECTORY_BUILDER_2D.max_range = 20.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

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

POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.huber_scale = 1e3

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)

return options

◆ オドメトリをオミットした結果

マ。。。マジかよ。。。 今までの全検証の中で一番まともな位置推定ができている。。。
かなりピクピクと忙しく動くが、動作を停止したタイミングの静止位置の検出精度はかなり高い。
ここまでの1週間の作業って、いったい。。。
◆ はじめに へ掲載のGIF画像のとおりの動きとなる。

アホな対応をしていることは理解している。
前回 動きがおかしかったのは、ロボット本体から発行しているオドメトリが原因だったのは明白だ。
ただ、1週間以上結果が出ないとモチベーションを維持するのが辛くなってくる。
問題点を細かく分解して短い期間で小さく結果を出し、本当にクリティカルではない問題は後で解決していく。

GIFは廊下に続くドアを閉め忘れたまま動画撮影してしまったため、改めてドアを閉めた状態で作成し直したリビングダイニングキッチンの全体地図が下図。
157.png
隅々まで移動していないので多少の粗さは残るが、上出来な気がする。
ガラスの部分からはレーザーが透過してしまっており、カーテンなどの布地の部分は波打っている。
冷蔵庫などのレーザーが真っすぐ反射しにくい家具部分はボンヤリとしている。
壁の部分はしっかりと直線で描画されているので十分な仕上がりな気がする。
部屋のあちこちに見える黒い物体は、ガキンチョが撒き散らしたまま片付けなかったオモチャたちの影とダイニングテーブルの足の部分。
一番上の壁が左下に向かってズレているように見えるのは三枚引き戸の部分で、引き戸1枚あたり 5cm ほどの段差が驚くほど正確に再現されている。

◆ 本日のまとめ

  • Google CartoGrapher によって、だいぶまともな地図が生成できた
  • 機体の動きがピクピクとせわしなく動くため、パラメータのチューニングを実施しなければ可視化時には格好が悪い
  • ナビゲーションの実施時にはオドメトリを真面目に取得する必要がありそうだ
  • LiDARの点群だけで地図生成できてしまうとは。。。 Google CartoGrapher の性能、恐るべし。
  • 助けを求めておきながら、自己解決してしまいました。すみません。。。
  • ネタがニッチすぎて地味ね

◆ 次回予告

ついに、ナビゲーションにトライするか、オドメトリの調整にトライしようと思う。
自動航行できるといいなぁ。

◆ 次回記事

RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_016日目_SLAM_Google CartoGrapher_オドメトリの調整と地図生成と位置推定 へ続く