LoginSignup
38

More than 3 years have passed since last update.

posted at

updated at

RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_008日目_SLAM_GMapping_LiDAR(A1M8)

zumo32u4 GitHub stars

◆ 前回記事

RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_007日目_SLAM_MONO-VO(単眼カメラ視差推定) の続き

◆ はじめに

前回 は単眼カメラによる視差推定(MONO-VO)という技法を使ってSLAMを行ってみた。
単眼カメラによる視差推定は、自己位置推定という観点ではアリなのだが、自分が移動した経路以外の周辺環境に関する情報が希薄に見えたので、周辺環境を考慮した自律航行にはなんとなく不向きに感じた。
今回は最もメジャーらしいアルゴリズム 「GMapping」 を試行してみる。
001日目 に記載のとおり、最終的にはPC非介在での自律航行が目的なので、軽量なアルゴリズムだといいなぁ。

チュートリアルひとつ動かすごとにことごとく意味を理解できず、あちこち迷走しながら進めたために長めの記事となる。
非公式な RaspberryPi + Raspbian Stretch による環境構築にこだわったため、とてつもなくしんどかった。。。
めちゃくちゃ長い記事になったので、先に試行結果の画像だけ公開する。
006日目 に実装したBluetoothによる遠隔操作を行えば、RaspberryPiごときでもリアルタイムにマップを生成することができる。
122.png
モデルデータ.png
0018_連携概要図_GMapping (1).png

自分のような素人でも頑張れば動かせたので、過去記事も含めて読み返せば必ず成功する記事を目指し、ハマりどころ、苦労した点も含め、包み隠さず漏れ無く全て記載させていただく。

今回 他記事 アルゴリズム 特徴
Gmapping 一番有名らしい、ROS対応、loop closure有り
009日目 Google Cartographer 精度が高そう、ROS対応、loop closure有り
010日目 Hector SLAM 精度がいまいちっぽい、loop closure無し
011日目 Karto SLAM 単眼カメラでも動く、ROS対応、Visual-SLAM
012日目 ORB-SLAM2 単眼カメラでも動く、Visual-SLAM
007日目 mono-vo 単眼カメラでも動く、Visual-SLAM
013日目 VINS-Mono 単眼カメラでも動く、ROS対応、Visual-SLAM、ドローン
CNN-SLAM 単眼カメラでも動く、3D、アイデアのメモ情報しか見当たらなかったため検証対象外とする
RGBD-SLAM-V2 RGB-Dカメラ専用、検証対象外とする

◆ 参考にさせていただいた記事、謝辞

下記、恐ろしく分りやすい。
最初に知っていれば。。。
クリエイティブ・コモンズライセンスということで有り難く共有させていただきます。

下記参考に読み始めた書籍。SLAMに特化していて素晴らしい。が、数式が読めない。。。
2018.05.02時点、著しく流通量が少ない。Amazon、楽天、Yahoo、メルカリ、全て撃沈。
品薄すぎて流通価格を¥1,000近く引き上げる輩が多い。
どうしてもすぐに適正価格で仕入れて読みたかったので書店に全力疾走した。

navigationパッケージ/slam_gmappingパッケージについてはこちらの記事が大変分り易い。
MoriKenさん

ROSのデバッグに関しては下記が分り易い。
H.Saitoさん

◆ 事前準備

いい加減、 source devel/setup.bash をターミナル起動のたびに入力するのが面倒になってきたので、Ubuntu側のターミナル起動時に自動的に実行されるように設定を仕込んでおく。

devel/setup.bashの自動実行設定
$ nano ~/.bashrc
下記を追記
source /home/<username>/catkin_ws/devel/setup.bash

◆ この記事のおおまかな流れ

大きく下記の流れで環境構築と試行を行いながら進めていく。

  1. 検証用環境整備(Ubuntu16.04上でのLidar利用準備と測域のシミュレーション)
  2. 検証環境上でのGMapping(Ubuntu16.04上でのGMappingシミュレーション)
  3. ロボット実機でのGMapping(RaspberryPi + Arduino + RPLidar A1M8)

◆ 検証用環境整備

【二発目】GMapping の検証に入る前に参考記事をベースに環境を整える。
このフェーズでの検証環境は下記。

■ 作業用PCなど

  • Ubuntu16.04
  • ROS kinetic
  • Qt
  • RPLidar A1M8 (SDK ver 1.5.7 / Firmware ver 1.20)

検証段階ではGUIで可視化して確認したいことも多いので、一時的にUbuntuPCを使用する。
RaspberryPi3(Raspbian Stretch)へも ROS(Headless版) は導入済み のため、検証が終わり次第そちらへ環境を移行する。

0019_連携概要図_Ubuntu.png

なお、ほかの環境構築は Raspbian Stretch+NCS(Neural Compute Stick)+YoloV2+Webカメラ+ROSによるリアルタイム複数動体検知環境の構築 と 002日目~007日目 の記事に記載の作業を実施済みであることを前提とする。

● Qtのオンラインインストーラをダウンロード

単なるエディタとしてしか使用しない見込みだが、教授のおすすめなのでとりあえず導入しておく。
エディタが不要なら導入する必要はない。
あまり関係ない話だが、このツール、ライセンスが微妙すぎる。。。
【Install Qt】 https://www.qt.io/download-qt-installer

● 統合開発環境QtCreatorのインストール(734.82MBの空き容量必要)

下記コマンドでオンラインインストーラを実行する。
x.x.x の部分はバージョン番号で、ダウンロードした時期により変わるため適宜読み替え必要。

オンラインインストーラの実行
$ cd ~/Downloads
$ chmod +x qt-unified-linux-x64-x.x.x-online.run
$ sudo ./qt-unified-linux-x64-x.x.x-online.run

インストール用ダイアログが起動してくるので、ほぼデフォルトのまま次へと進んでいく。
途中、SKIP可能なところはSKIPする。

途中、コンポーネントの選択画面で下記のとおり選択する
65.png

インストール進行中の画面
66.png

終わり。
67.png

起動するとこのような感じ。
68.png

● 格安LiDAR RPLidar A1M8 のキッティング

< RPLidar A1M8の外観 >
DSC_0062.png DSC_0063.png
税込み ¥25,000- 外装が無駄にオシャレ、内装は型抜きスポンジで厳重・丁寧に梱包されていた。
「仕事をかなり頑張ったから・・・」 とかなんとか言って、我が家の大蔵大臣から無理やり勝ち取った、なけなしの小遣いをはたいて買った。

・・・高いわ!!!

RaspberryPi3とほぼ同じ大きさ。

ちなみに、あちこちのブログや研究記事で紹介されている機器は下記。
通称URGシリーズ。
税込み ¥200,000- ほどもする高価なものなのでパンピーには到底手が出せない。
さすが国産。 研究用途でもないのに2ケタ万円も銭出せる庶民がどこにいるって言うんだろうか。
ただし、性能はむちゃくちゃ高い。実際に動いている様をこの目で見てきた。
おそらく、国内の産業用途には確実にこちらの姉妹機器が採用されていることだろう。

76.png

LiDARに関してはVelodyne社がかなり開発先行しているっぽい。
下記はモーター駆動しないのでかなりの長寿命 なおかつ 高耐久。
早く1万円切ってほしい。
推定価格 数十万円也。

77.png

下記のように、わずか¥4,000でLiDARを自作した強者もいる。
気合と根性がある方は挑戦してみてはいかがだろうか。

余談はさておき、作業を進める。
srcフォルダ直下で RPLidar A1M8 のROSパッケージをCloneしてビルドする。

公式のGithubリポジトリ上のSDKバージョンはVer1.5.7。
PullRequestが6件近く半年以上も無視されていて、アップデートが止まっているようだったので、Forkして 自分のリポジトリ上 でMergeしておいた。
他の方々があげたプルリクは主に、安定性向上と利便性向上。
どうせ無視されるのが分かっているので自分からはプルリクをあげない。

そうこうしているうちに気づいた。
Gitは放置しているクセに 自社ページ(中国語) に最新Ver1.6.0のSDKとファームウェアVer1.24をアップしてござる。
ファームウェアはご丁寧に「.exe」で固められている。 って、オイッ!
さすが。仕事が雑い。イラッとする。
そして、WindowsPCを使用してアップデートを試してみたが、成功しなかった。
現状のFirmwareバージョンが Ver1.20 だと Ver1.24 にすらアップデートもさせてくれない仕様のようだ。
Lidarの購入時期によってアップデートできる機器とできない機器があるようなので注意。
現状最新のA1M8のリビジョンは、A1M8-R3 らしいので、販社から購入される場合は しっかりと確認してから の購入をオススメする。
測定周期が4Kに、測域距離が6mから12mへ倍増するらしいので誠に残念。(甚だ懐疑的だけど)
最新VerのSDKは色々と問題が出そうなので今回は見送り、Ver1.5.7のまま進めるものとする。

rplidar_rosのダウンロードとビルド
$ cd ~/catkin_ws/src
$ git clone https://github.com/PINTO0309/rplidar_ros.git
$ cd ..
$ catkin_make

masked.png

masked.png

rviz(可視化ツール) をインストールする。

rvizのインストール
$ sudo apt install ros-kinetic-rviz

LiDARとPCをmicroUSB-Aケーブルで接続する。
USBへのアクセス権限割当が終わっていない場合は下記を実施。

USBへのアクセス権限割当_"nnn"の部分は可変の数字、各自読み替え必要
$ sudo addgroup <username> dialout
$ sudo usermod -a -G dialout <username>
$ sudo ls /dev/ttyU*
上記で表示された名前で下記以降を置き換えてコマンドをたたく。
$ sudo chmod 666 /dev/ttyUSB<nnn>
$ cd /etc/udev/rules.d
$ sudo touch ttyUSB<nnn>rule.rules
$ sudo nano ttyUSB<nnn>rule.rules

KERNEL=="ttyUSBnnn", MODE="0666"

接続した瞬間、通電と同時にレーザー照射部が自動的に回り始める。
DSC_0064.png

rviz(可視化ツール) のデモプログラムを起動する。

rvizデモプログラムの起動
$ roslaunch rplidar_ros view_rplidar.launch

MASTER の起動と rplidar_ros パッケージの起動が同時に行われた。
1.png

と、同時にrvizアプリケーションが起動してきた。
おぉっ! ほぼ何もしていないのにLiDARで測域できてるよ!
カッコいい〜〜!!
75.png

rvizは3次元空間に表示されていて、黒い画面上をマウスでドラッグすると色々な角度から測域状況を見ることができるようだ。
デモやテストでは可視化されていないと何が起こっているのか訳が分からないが、RaspberryPiのHeadless版ROSへ搭載するときには点群の情報だけ貰えれば良いので、rviz(可視化ツール)は不要になる想定。

では、rvizを起動せずにPublisherのみを起動してみる。
まずはMasterを起動する。

Masterの起動
$ roslaunch rplidar_ros rplidar.launch

Masterと共に、 rplidarNode というPublisherっぽいものも起動したようだ。

masked.png

別のターミナルを起動して下記を実行し、Subscriberを起動する。

Subscriberの起動
$ rosrun rplidar_ros rplidarNodeClient

猛烈な勢いでレーザースキャンした結果をSubscribeし始めた。
正直言って、数字の羅列なので現時点では意味不明。
masked.png

では、どういうTOPICなのかを確認してみる。
まずはTOPICの一覧を表示。

TOPICの一覧表示
$ rostopic list

scan という名前のTOPICが発行されていることが分かった。
82.png

では、scan という名の TOPIC の内容がどういうものか確認してみる。

TOPICの内容表示
$ rostopic echo /scan

見出しが表示されたので、先ほどよりは情報が読みやすくなった。
今度はなんとなく雰囲気が伝わる。
masked.png

では、チュートリアルを参照して、ひとつづつ意味を調べてみる。

● LiDARスキャンデータの意味を調べる

参照した記事は下記。
【wiki.ros.org】 http://wiki.ros.org/rplidar

No. データ 意味
1 [std_msgs/Header] seq uint32 データの順序番号
2 [std_msgs/Header] stamp.secs time タイムスタンプ
起源からの経過時間[秒]
3 [std_msgs/Header] stamp.nsecs time タイムスタンプ
起源からの経過時間[ナノ秒]
4 [std_msgs/Header] frame_id string 0:no frame、1:global frame、それ以外:TFフレーム名
5 angle_min float32 スキャンの開始角度[θ]
6 angle_max float32 スキャンの終了角度[θ]
7 angle_increment float32 測定間の角度間隔
8 time_increment float32 測定間隔[秒]
9 scan_time float32 スキャン間隔[秒]
10 range_min float32 最小測定距離[メートル]
11 range_max float32 最大測定距離[メートル]
12 ranges float32[] 距離[メートル]、「inf」 は 「infinite(無限大)」 の略かな?おそらく測定不能
13 intensities float32[] 強度データ (LiDARによって有無アリ) A1M8は出力されるようだ

角度(angle_increment) と 距離(rages)、スキャン間隔(scan_time) あたりが重要なのかな。
ここで、先ほどまで意味不明だった数字の羅列の意味がようやく分かってきた。
0016_LiDAR.png

色々調べているうちにモーターの動作スピードを調整できるらしいことが分かったので、SDK内のC++のソースを探っていたところ、PublisherのNode初期化ロジックで下記のような記述を見つけた。

catkin_ws/src/rplidar_ros/src/node.cpp
    ros::NodeHandle nh;
    ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
    ros::NodeHandle nh_private("~");
    nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); 
    nh_private.param<int>("serial_baudrate", serial_baudrate, 115200);
    nh_private.param<int>("motor_pwm", motor_pwm, DEFAULT_MOTOR_PWM);
    nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
    nh_private.param<bool>("inverted", inverted, false);
    nh_private.param<bool>("angle_compensate", angle_compensate, true);

DEFAULT_MOTOR_PWM だと? どれぐらいのPWM周波数に設定されているのかを調べる。
30分後、見つけた。

catkin_ws/src/rplidar_ros/sdk/include/rplidar_cmd.h
#define MAX_MOTOR_PWM               1023
#define DEFAULT_MOTOR_PWM           660

機器の仕様上、6Hz 〜 10Hz のレンジを取りうるということなので、 660 っていうことは、6.6Hz かな。
すぐ1行上に MAX_MOTOR_PWM 1023 という数字が見えているので、10Hz最大スピードまでブーストしてみたい。

起動時の launch ファイルで初期化パラメータを指定できると踏んだうえで改変してみる。

編集前_catkin_ws/src/rplidar_ros/launch/rplidar.launch
<launch>
  <node name="rplidarNode"          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="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  </node>
</launch>
編集後_catkin_ws/src/rplidar_ros/launch/rplidar.launch
<launch>
  <node name="rplidarNode"          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="motor_pwm"           type="int"    value="1023"/>
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  </node>
</launch>

先ほどと同じ手順で起動してみるが・・・うむ、全く違いが分からない!!
とりあえずデフォルトへ戻す。

◆ GMapping

ようやく本題の GMapping の試行に入る。
GMapping を実行する場合は、 navigationパッケージslam_gmappingパッケージ というものを導入するのがセオリーらしい。

● Ubuntu開発機とシミュレータによる試行

このフェーズの作業環境・実行環境は下記。

■ 作業PC

  • Ubuntu16.04
    • ロボットシミュレータ「turtlebot」の実行
    • 「turtlebot」公開トピックによるrviz可視化

0020_連携概要図_Ubuntu.png

* ROSの navigationパッケージslam-gmappingパッケージ をインストール

そもそも唐突に navigationパッケージ って、なんだろう?

参考記事の師曰く
自己位置推定、大域的な経路計画、局所的な動作計画など、
地図ベースの自律走行に必要なノードを含むパッケージ群で、
世界中のROS開発者によって作成・管理されている。
世界中の英知の塊。

人類の英知、と来たか。
何やら凄そう。これは入れないわけにはいかないだろう。
SLAM、駆逐してやる!
ということで、下記でインストールする。

242個の新規インストールパッケージがあり662MBも消費する
$ sudo apt install ros-kinetic-navigation

次に、slam_gmappingパッケージ をインストールする。

1.5MB程度なので一瞬で終わる
$ sudo apt install ros-kinetic-slam-gmapping

* GMappingチュートリアルの実行

チュートリアルは下記を追加参照した。

まずは出来合いのサンプルデータをもとに地図の可視化部分のみを味見してみる。
チュートリアルで公開されている bagファイル(シミュレーション記録データ) をダウンロードする。

bagファイルのダウンロード、好きなパスで実行
$ wget http://wiki.ros.org/slam_gmapping/Tutorials/MappingFromLoggedData?action=AttachFile&do=get&target=basic_localization_stage.bag
ターミナルを起動してMaster実行
$ roscore

use_sim_time true をノード起動前に実行しておくと、バーチャル世界の時間経過をダミーでシミュレートしてくれるようになるらしい。
下記コマンドを実行する。

別のターミナルでgmappingパッケージのslam_gmappingモジュールにbase_scanトピックをSubscribeさせる
$ rosparam set use_sim_time true
$ rosrun gmapping slam_gmapping scan:=base_scan

シミュレートされているバーチャル世界の時間経過を元に、LiDARの点群データ等を再生する。

シミュレーション記録済みのLiDAR点群記録データをプレイバックし、マップ生成コマンドを実行
$ rosbag play --clock <bagファイルのダウンロード先パス>/basic_localization_stage.bag

35秒ほどシミュレーションデータの再生待ちが必要

$ rosrun map_server map_saver -f map

rosrun map_server 実行時のカレントパスに map.pgm と map.yaml ファイルが生成される。
Ubuntuなら map.pgm をダブルクリックするだけでイメージビューアが自動的に起動するようだ。

どこの誰の家だか知らないが、シミュレーションに記録されていた点群データを元に、とある一室のマップが生成された。
85.png

これで、なんとなく雰囲気はつかめた。

* シミュレータによる独自の地図データ生成

では、実機のロボットを使用してマップを生成する前に、感覚を養うため地図生成をシミュレータ(turtlebot)で試行してみる。
下記コマンドで、シミュレータ turtlebot-gazebo をインストールする。
ROSはシミュレータがあらかじめ用意されているあたりが親切丁寧で誠に助かる。

!!! 注意 !!!
下記手順を純粋に踏むと、 PYTHONPATH に Python2.7 のパスが勝手に追加されることによって、Python3系のOpenCVへのリンクがとれなくなる。
よって、python3系 cv2.so へのシンボリックを自分で作って、OpenCV を使用する Python3系自作プログラムの実行パスへコピーしたりしないと、バージョンの競合が発生して今まで正常に動いていたOpenCVを使用するPython3系プログラムが軒並み動かなくなる。
turtlebot-gazebo の導入は諸刃の剣のようなのでゴチャゴチャするのが面倒くさい場合や対処に自身が無い場合は、PC環境を別に仕立てたほうが良さそうだ。

自分は今まで散々環境構築に時間を掛けてきてハマりちらかしてきただけあって、自力でなんとかできる知識レベルにありそうだったので、そのまま turtlebot-gazebo を導入してみる。
案の定、OpenCVを使用するPython3系のプログラムが普通には動かなくなったことを特筆しておく。

turtlebotの導入
$ sudo apt update
$ sudo apt install ros-kinetic-turtlebot-gazebo
$ sudo apt install ros-kinetic-turtlebot ros-kinetic-turtlebot-apps ros-kinetic-turtlebot-interactions ros-kinetic-turtlebot-simulator ros-kinetic-kobuki-ftdi ros-kinetic-ar-track-alvar-msgs
turtlebotの起動
$ roslaunch turtlebot_stage turtlebot_in_stage.launch

masked.png

rviz が起動して turtlebot が平面の左上あたりにマッピングされ、サンプルのマップと共に表示された。
直感的に黒い線が壁かな。
91.png

turtlebotはキーボード操作用のノードを起動しないと操れないらしい。
下記のコマンドを実行する。

turtlebotをキーボードで動かすノードを起動
$ roslaunch turtlebot_teleop keyboard_teleop.launch

masked.png

下記のキーマッピングでキーボードから操作可能らしい。
0017_turtlebot.png

では、turtlebot を動かしながら、turtlebotに実装されているセンサーのスキャンデータを rosbag にシミュレーションデータとして一時的に記録してみる。
rosbagコマンドは下記のように実行するようだ。
test.bag というファイルに、 scanトピックtfトピック を記録し続ける。

rosbagによるシミュレーション記録データの生成
$ rosbag record -O test.bag /scan /tf

自由に turtlebot を移動させること3分。。。

Ctrl + c で rosbag の記録動作を終了させる。
rosbag実行時のカレントパスに test.bag というファイルが生成される想定。
「~/catkin_ws/」直下で実行したが、はたして・・・
92.png

ふむ、ちゃんと生成されているようだ。

記録した test.bag の情報を覗くためのコマンドが用意されているようだ。
念の為実行してみる。

シミュレーション記録データの情報参照用コマンド
$ rosbag info test.bag

masked.png

何やら日時が極めておかしなことになっている。
が、ちゃんと scanトピックtfトピック が記録されているようにも見える。
では、直前のセクションで実施したのと同じように、記録されたシミュレーションデータを基にマップを生成してみる。

Masterの起動
$ roscore
別のターミナル上でslam_gmappingを起動
$ rosparam set use_sim_time true
$ rosrun gmapping slam_gmapping
別のターミナル上でtest.bagをプレイバック
$ rosbag play --clock test.bag
3分間ものbagデータを記録してしまったため、カップラーメンに湯を足しながら3分間のプレイバック完了を待つ

プレイバック中、slam_gmappingを実行したターミナル上には猛烈な勢いでログが出力され続けた。
いったん、意味内容の理解は置いておく。
masked.png

地図生成のコマンドを実行する。

地図を生成するコマンド
$ rosrun map_server map_saver -f map2
$ rosparam set use_sim_time false

一瞬で終わった。
masked.png

ちゃんと map2.pgm ファイルが生成されているかな?
見ると、 map2.pgm と併せて map2.yaml という定義ファイルっぽいものも同時に生成されている。
98.png

map2.yaml を見てみる。が、現時点では意味が分からない。
99.png

では、目的の地図はどんな感じに生成されているかな?
map2.pgm をダブルクリックで開いてみる。
左ハシ中央の位置から右上スミの位置まで turtlebot を移動させた結果生成された地図なので、移動経路上からレーザースキャンが届く範囲の地図が生成されており、思っていたよりまともに生成されている感じ。
赤い丸と赤い線は後からペイントソフトにて手で書き足した経路イメージ。
100.png

● RaspberryPi(Raspbian Stretch) + Arduino + Ubuntu開発機を使用した実機による地図データ生成

* 実機準備

では、実機の ロボット と LiDAR を使用してマップ生成を試行してみる。
このフェーズの作業環境・実行環境は下記。

■ ロボット

  • Arduino 32u4

    • 加速度、方位、エンコーダ値、ジャイロの取得
    • 左右のモーター制御
  • RaspberryPi3(Raspbian Stretch)

    • ロボット操作コマンドのBluetooth中継
    • オドメトリ/IMUデータ加工・Publish
    • LiDARの点群データ加工・Publish
    • ROSのMaster起動
    • navigationパッケージ と GMappingパッケージ起動
  • RPLidar A1M8 (SDK ver 1.5.7 / Firmware ver 1.20)

    • 点群データ採取

■ 作業PC

  • Ubuntu16.04
    • Arduinoスケッチのコンパイル+Arduinoへのスケッチ書き込み
    • RaspberryPiからの公開トピックによるrviz可視化

■ ロボット操作PC

  • Windows10 Pro
    • TeraTermによる、ロボットへの前後進、右左折、停止、指示操作

0018_連携概要図_GMapping (1).png

まず、作業用PC(Ubuntu)側でrvizにより地図生成過程をリアルタイムに可視化するため、作業用PCがRaspberryPi上で稼働するMasterを参照できるように設定変更する。
一度、現在起動しているROSまわりのターミナルを全て終了する。
そして、下記コマンドでUbuntuの環境変数を一時的に追加する。

Ubuntu側で実行
$ export ROS_MASTER_URI=http://raspberrypi.local:11311/

または

$ export ROS_MASTER_URI=http://<RaspberryPiのIPアドレス>:11311/

RaspberryPi上で起動しているMasterがUbuntu側から正常に参照できるかを確認する。
とりあえず簡単なコマンドでトピックが見えることを確認する。

RaspberryPi側で実行
$ roscore
Ubuntu側で実行
$ rostopic list

masked.png

TOPICの一覧が取得できたので、しっかりとUbuntu側からRaspberryPi側のMasterが参照できるようになったようだ。

* ROSの navigationパッケージ と slam-gmappingパッケージ をRaspberryPiにインストール

前のフェーズでUbuntuに対して実施した作業の一部をRaspberryPi(Raspbian Stretch)でも同様に行う。
Raspbian Stretchでは、navigation、slam-gmapping 共に aptコマンドで素直に導入できなかったため、依存パッケージの手動インストールとソースビルドでインストールする。

catkin_makeを通すのに丸6日間を連日徹夜まがいで浪費する。
自分のようにスキルが激低だと、エラー原因を突き止めるのも、解消するのも地獄だった。
navigationパッケージの手動ビルドは、正常に終わるパターンでも、純粋な待ち時間だけで丸1日掛かる。
ARMのCPU かつ メモリ1GBの端末でやるもんじゃないな、これ。

将来的にdeb化しておかないと干からびそうだ。
時間を大きく費やした要因は、

  1. 大量にあるパッケージ間の依存関係の解消作業
  2. コンパイラバージョンとパッケージバージョン間の整合作業
  3. CMAKE_PREFIX_PATHの不整合の特定と解消
  4. 最大SWAPエリア容量(32bitOSは最大2GBまで)を超えないようにビルドの並列数を絶妙に調整

特にタチが悪いのは 3. で、ビルドが中盤に差し掛かったところで初めてC++のビルドエラーが発生する。
OpenCVのビルドだけで10日間を費やしたのを思い出す。
正直、navigationパッケージのビルドを通す奮闘記録だけで一記事書ける気がする。
実行時にSegmentation Faultが発生しないことだけを願う。

<参考:Debianパッケージ検索用Webページ "パッケージ名"で検索>
https://www.debian.org/distrib/packages

RaspberryPiへnavigationパッケージをインストール
$ sudo apt update;sudo apt upgrade
$ sudo apt install -y libbullet-dev libsdl-dev libsdl2-2.0 libsdl2-dev libsdl-image1.2-dev libyaml-cpp0.3-dev libyaml-cpp0.3v5

$ cd ~
$ sudo apt install cmake-curses-gui
$ git clone -b debian/kinetic/bfl https://github.com/PINTO0309/bfl-release.git
$ cd bfl-release;mkdir build;cd build
$ ccmake ..

cキーを押す
30秒ほど待機する
eキーを押す

下記のように設定。
masked.png

RaspberryPiへnavigationパッケージをインストール
cキーを押す
eキーを押す
gキーを押す

$ make -j3
$ sudo make install

$ cd ~
$ sudo apt install -y liborocos-kdl-dev liborocos-kdl1.3 libflann1.9 libflann-dev \
libpcl-apps1.8 libpcl-common1.8 libpcl-dev libpcl-doc libpcl-features1.8 \
libpcl-filters1.8 libpcl-io1.8 libpcl-kdtree1.8 libpcl-keypoints1.8 libpcl-ml1.8 \
libpcl-octree1.8 libpcl-outofcore1.8 libpcl-people1.8 libpcl-recognition1.8 \
libpcl-registration1.8 libpcl-sample-consensus1.8 libpcl-search1.8 \
libpcl-segmentation1.8 libpcl-stereo1.8 libpcl-surface1.8 libpcl-tracking1.8 \
libpcl-visualization1.8 pcl-tools libvtk6-dev libvtk6-java libvtk6-jni \
libvtk6-qt-dev libvtk6.3 libvtk6.3-qt python-vtk6 tcl-vtk6 vtk6 vtk6-doc \
vtk6-examples libpoco-dev libpococrypto46 libpocodata46 libpocodatamysql46 \
libpocodataodbc46 libpocodatasqlite46 libpocofoundation46 libpocomongodb46 \
libpoconet46 libpoconetssl46 libpocoutil46 libpocoxml46 libpocozip46

$ cd /
$ sudo wget https://github.com/PINTO0309/uEye_4.90.0_Linux_Arm64hf/raw/master/uEye_4.90.0_Linux_Arm64hf.tgz
$ sudo tar -xzvf uEye_4.90.0_Linux_Arm64hf.tgz
$ sudo rm uEye_4.90.0_Linux_Arm64hf.tgz
$ sudo /usr/local/share/ueye/bin/ueyesdk-setup.sh

$ sudo nano /etc/dphys-swapfile
CONF_SWAPSIZE=2048
$ sudo /etc/init.d/dphys-swapfile restart swapon -s

$ export CMAKE_PREFIX_PATH=/opt/ros/kinetic:/home/pi/catkin_ws/devel:/home/pi/catkin_ws/install
$ cd ~/catkin_ws/src
$ git clone -b kinetic-devel https://github.com/PINTO0309/geometry2.git; \
git clone https://github.com/PINTO0309/dynamic_reconfigure.git; \
git clone https://github.com/PINTO0309/laser_geometry.git; \
git clone https://github.com/PINTO0309/navigation_msgs.git; \
git clone https://github.com/PINTO0309/perception_pcl.git; \
git clone https://github.com/PINTO0309/pcl_msgs.git; \
git clone https://github.com/PINTO0309/nodelet_core.git; \
git clone https://github.com/PINTO0309/bond_core.git; \
git clone -b kinetic-devel https://github.com/PINTO0309/pluginlib.git; \
git clone https://github.com/PINTO0309/class_loader.git; \
git clone https://github.com/PINTO0309/image_common.git; \
git clone https://github.com/PINTO0309/vision_opencv.git; \
git clone https://github.com/PINTO0309/geometry.git; \
git clone https://github.com/PINTO0309/roslint.git; \
git clone https://github.com/PINTO0309/navigation.git

$ cd ..
$ catkin_make -j1
$ sudo reboot

次に、slam_gmappingパッケージ をインストールする。

RaspberryPiへslam_gmappingパッケージをインストール
$ cd ~/catkin_ws/src
$ git clone https://github.com/PINTO0309/openslam_gmapping.git
$ git clone https://github.com/PINTO0309/slam_gmapping.git

$ cd ..
$ catkin_make -j1

次に、rplidar_rosパッケージ をインストールする。

RaspberryPiへrplidar_rosパッケージをインストール
$ cd ~/catkin_ws/src
$ git clone https://github.com/PINTO0309/rplidar_ros.git
$ cd ..
$ catkin_make

次に、作業用PC(Ubuntu)へrvizのPluginを導入しておく。
見た目が良くなる、っぽい。
導入しておかないとrviz起動時にエラーとなる。

UbuntuでrvizのPluginを導入
$ cd ~/catkin_ws/src
$ git clone https://github.com/PINTO0309/visualization_tutorials.git
$ git clone https://github.com/PINTO0309/joint_state_publisher.git
$ git clone https://github.com/PINTO0309/robot_state_publisher.git
$ catkin_make

* RaspberryPi と Arduino へのプログラムの実装

参考にした書籍によると、地図生成を行うにはLiDARによる点群データの他に、IMUとオドメトリの情報が併せて必要、とのことだ。
では早速、IMU および オドメトリをZumo32u4から取得するため、Zumoに最初から搭載されているLSM303チップ (3軸加速度計と3軸磁力計、3軸ジャイロ計を同梱) を使用し、加速度と方角を取得するプログラムを作成する。

前回までの経験値 (5日目6日目) により、モーター制御を行いつつIMU/オドメトリをPublishするためのArduino用スケッチ 並びに Windows作業用PCとRaspberryPi間のシリアル通信 および RaspberryPiとArduino間のrosserial連携を行うプログラムを書いて、ArduinoとRaspberryPiへそれぞれデプロイする。

===ココからは rosserial が正常に動作しなくなったときの奮闘記録。環境構築手順上は無視して良い。===

またしてもハマった。。。
今度は rosserial がまともに動かない。。。
6日目 のとおりにやっているはずなのに、何故か動かない。
下図のような見たことのないエラーとなる。
チュートリアル的にプログラムを書いたときと今回の違いと言えば、加速度と方位、ジャイロをArduinoからRaspberryPiへPublishするように変更したことぐらい。
まさかとは思うが、Publishする情報量が多すぎてArduinoがパンクした、とか?
Note: publish buffer size is 512bytes の表示がすごく気になる、と、野生の直感が叫んでいる。。。
masked.png

表示されたエラーメッセージをキーワードにググってみると下記の記事を見つけた。
今回作成したプログラムは、1Publish あたり 80バイト前後を送出しているので、512バイトのバッファだと10個分すらもバッファリングできない。
how to increase rosserial buffer size ROS ANSWERS

対策は、ArduinoIDEでスケッチをビルドしているUbuntu作業PC側の rosserialライブラリ のロジックを改変せよ。とのことだ。
対象は ros.h ファイルらしい。
Publishする際にArduino内部で確保可能なバッファサイズを拡張しなくてはならない。
自分はUbuntuを作業PCとしているが、 ros.h が、どこにあるのか分からないので検索してみる。

作業用PC側(Ubuntu)でros.hを捜索するコマンド
$ sudo find / -name ros.h

masked.png

いくつか候補が表示されているが、とりあえずArduinoIDEに一番近そうな下記。 /home/<username>/Arduino/libraries/Rosserial_Arduino_Library/src/ros.h

これにあたりをつけて修正してみる。

ros.hの修正
$ cd /home/<username>/Arduino/libraries/Rosserial_Arduino_Library/src
$ cp ros.h BK_ros.h
$ sudo nano ros.h
修正前
#else
  typedef NodeHandle_<ArduinoHardware, 25, 25, 512, 512, FlashReadOutBuffer_> NodeHandle;
修正後
#else
  typedef NodeHandle_<ArduinoHardware, 15, 15, 128, 1024, FlashReadOutBuffer_> NodeHandle;

ros.h の修正後にArduinoIDEを再起動し、今一度Arduinoへ同じスケッチを書き込んで、RaspberryPi側で rosserial を起動してみる。

Masterを起動
$ roscore
rosserialの起動
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

masked.png

正常に起動した。
publish buffer が 1024 bytes に増えたようだ。
引き換えに subscribe buffer が 512 bytes から 128 bytes に減ってしまった。
まぁ、動くなら良しとする。
注意点は、Arduino基板の種類によってバッファ確保が可能な全体サイズが異なるようなので、環境ごとに絶妙な数字の組み合わせを探る必要があること。
今回自前で作成したトピック、 /zumo32u4/command/zumo32u4/sensorval が Master に登録されていることを確認する。

TOPICの確認
$ rostopic list

109.png

また、Arduino側からPublishした、時間、加速度、方位、左右モータースピード、エンコーダ値、ジャイロ、が、きちんとRaspberryPi側でSubscribeできた。

ArduinoからPublishされたsensorvalトピックの確認
$ rostopic echo /zumo32u4/sensorval

110.png

では、操作用コマンドのPublish確認はひとまず置いておいて、IMU、オドメトリのPublishをRaspberryPiのPythonから実行できるか試してみる。

RaspberryPiからIMU/オドメトリをPublishするPythonプログラムの起動
$ rosrun zumo32u4 zumo32u4.py

masked.png

エラーにならずに動き始めたようだ。
遠隔操作用のBluetoothを非接続のままテストしているため、シリアル接続に失敗したことを知らせる警告が表示されているが、とりあえず現段階のテストでは無視する。
WindowsPCとRaspberryPiをBluetoothで接続すれば表示されなくなる。

===ココまでは rosserial が正常に動作しなくなったときの奮闘記録。環境構築手順上は無視して良い。===

長くなってしまったが、上記問題解消済みのプログラム一式(スケッチとPython)は Github へコミットしてあるが、作業用PC(Ubuntu)の ros.h 改変、作業用PC(Ubuntu)へのCloneとArduinoへのスケッチ書き込み および RaspberryPiへのCloneとプログラムのビルドの手順は下記にそのまま残しておく。

まずは、作業用PC(Ubuntu)の rosserialライブラリ ros.h を改変する。

ros.hの修正、ros.hのファイルパスは環境によって異なる可能性はある
$ cd /home/<username>/Arduino/libraries/Rosserial_Arduino_Library/src
$ cp ros.h BK_ros.h
$ sudo nano ros.h

#else
  typedef NodeHandle_<ArduinoHardware, 15, 15, 128, 1024, FlashReadOutBuffer_> NodeHandle;

作業用PC(Ubuntu)にてプログラム一式をCloneする。

Ubuntuへプログラム一式をClone
$ cd ~/catkin_ws/src
$ git clone https://github.com/PINTO0309/zumo32u4.git

UbuntuとArduinoをmicroUSBケーブルで接続したのち、上記でCloneしたプログラム一式の中の zumo32u4/zumo32u4arduino.ino をArduinoIDEで開く。

モーター制御とセンサー値を取得してPublishするArduino用スケッチ
zumo32u4arduino.ino
#define USE_USBCON
#include <ros.h>
#include <std_msgs/String.h>
#include <Wire.h>
#include <LSM303.h>
#include <Zumo32U4.h>

long timer=0;              // Elapsed time since program started (milli second)
int vright = 0;            // Left Morter velocity (speed of motor)
int vleft = 0;             // Right Morter velocity (speed of motor)
int basespeed = 50;        // Base speed of Morter (Effective Range: 1 - 350)
long positionLeft  = 0;    // For encoder verification
long positionRight = 0;    // For encoder verification
long newLeft, newRight;    // Value of Encorder
std_msgs::String str_msg;  // Sensor value to be published

LSM303 compass;            // Magnetometer
L3G gyro;                  // Gyrometer
Zumo32U4Motors motors;     // Morter
Zumo32U4Encoders encoders; // Encoder
ros::NodeHandle nh;        // NodeHandler of ROS

void motorcontrol(const std_msgs::String& cmd_msg)
{

  // I : forward    ,  , : backward
  // L : turn right ,  J : turn left
  // S : stop

  String cmd = "";

  if (strlen(cmd_msg.data) != 0)
  {
    cmd = cmd_msg.data;
  }

  if (cmd == "i")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    motors.setSpeeds(basespeed, basespeed);
    vleft = basespeed;vright = basespeed;
  }
  else if (cmd == ",")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    motors.setSpeeds(-1 * basespeed, -1 * basespeed);
    vleft = -1 * basespeed;
    vright = -1 * basespeed;
  }
  else if (cmd == "l")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    motors.setLeftSpeed(basespeed + 50);
    vleft = basespeed + 50;
    vright = 0;
  }
  else if (cmd == "j")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    motors.setRightSpeed(basespeed + 50);
    vleft = 0;
    vright = basespeed + 50;
  }
  else if (cmd == "s")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    vleft = 0;
    vright = 0;
  }

  int16_t newLeft = encoders.getCountsLeft();
  int16_t newRight = encoders.getCountsRight();

  if (!(encoders.checkErrorLeft()) && !(encoders.checkErrorRight())) {
    if (newLeft != positionLeft || newRight != positionRight) {
      positionLeft = newLeft;
      positionRight = newRight;
    }
  }

}

ros::Subscriber<std_msgs::String> sub("/zumo32u4/command", motorcontrol);
ros::Publisher chatter("/zumo32u4/sensorval", &str_msg);

void setup()
{
  //Serial.begin(9600);     // Debug Print

  Wire.begin();

  nh.initNode();           // Init ROS Node
  nh.advertise(chatter);   // Init ROS Publisher
  nh.subscribe(sub);       // Init ROS Subscriber

  compass.init();          // Init magnetometer
  compass.enableDefault();

  gyro.init();             // Init gyrometer
  gyro.enableDefault();
}

void loop()
{
  compass.read();   // Read magnetometer
  gyro.read();      // Read gyrometer
  timer = millis();
  String s = "";
  s += timer;       // [0]  Elapsed time since program started (milli second)
  s += ',';
  s += compass.a.x; // [1]  Accelerometer.x
  s += ',';
  s += compass.a.y; // [2]  Accelerometer.y
  s += ',';
  s += compass.a.z; // [3]  Accelerometer.z
  s += ',';
  s += compass.m.x; // [4]  Magnetometer.x
  s += ',';
  s += compass.m.y; // [5]  Magnetometer.y
  s += ',';
  s += compass.m.z; // [6]  Magnetometer.z
  s += ',';
  s += vleft;       // [7]  Left Morter velocity (speed of motor)
  s += ',';
  s += vright;      // [8]  Right Morter velocity (speed of motor)
  s += ',';
  s += newLeft;     // [9]  Left Morter odometry (Rotation angle of motor)
  s += ',';
  s += newRight;    // [10] Right Morter odometry (Rotation angle of motor)
  s += ',';
  s += gyro.g.x;    // [11] Gyrometer.x
  s += ',';
  s += gyro.g.y;    // [12] Gyrometer.y
  s += ',';
  s += gyro.g.z;    // [13] Gyrometer.z

  //Serial.println(s);  // Debug Print

  str_msg.data = s.c_str();
  chatter.publish(&str_msg);
  nh.spinOnce();
  delay(1);
}

「→」ボタンでArduinoへスケッチを書き込む。
116.png

RaspberryPiにてプログラム一式をCloneして、ROS連携用Pythonプログラムをビルドする。

RaspberryPiへプログラム一式をCloneしてビルド
$ cd ~/catkin_ws/src
$ git clone https://github.com/PINTO0309/zumo32u4.git
$ cd ..
$ catkin_make

Bluetoothによるコマンド中継、Arduinoから送信されたセンサーデータをSubscribe、IMU/オドメトリを加工してPublish するPythonプログラム
zumo32u4.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import traceback
import serial
from math import sqrt, cos, sin
import rospy
import tf
from time import sleep
from threading import Lock
from geometry_msgs.msg import Twist, Pose
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from std_msgs.msg import String


class Zumo:
    def __init__(self):
        self.DIAMETER=0.038  #Meter
        self.INTERAXIS=0.084 #Meter
        self.COUNT=48
        self.temps=0
        self.theta=0
        self.odomR=0
        self.odomL=0 

        try:
            self.PORT = rospy.get_param('BLUETOOTH_PORT') 
        except:
            rospy.set_param('BLUETOOTH_PORT',"/dev/rfcomm0")
            self.PORT = rospy.get_param('BLUETOOTH_PORT')

        try:
            self.BAUDRATE = rospy.get_param('BLUETOOTH_BAUDRATE') 
        except:
            rospy.set_param('BLUETOOTH_BAUDRATE', "115200")
            self.BAUDRATE = rospy.get_param('BLUETOOTH_BAUDRATE')

        self.TIMEOUT = 0.01
        self.lock = Lock()
        self.sensorvalue = list()
        self.speed = []
        self.angle = []

        # Init Odometry
        self.o = Odometry()
        self.o.pose.pose.position.x = 0.0
        self.o.pose.pose.position.y = 0.0
        self.o.pose.pose.position.z = 0.0
        self.o.pose.pose.orientation.z = 0.0        
        self.o.header.stamp = rospy.Time.now()
        self.o.header.frame_id = "map"
        self.o.child_frame_id = "base_link"

        # Init IMU
        self.p = Imu()
        self.p.header.stamp = rospy.Time.now()
        self.p.header.frame_id = "imu"

        try:
            self.ser = serial.Serial(self.PORT, self.BAUDRATE, timeout = self.TIMEOUT)
            sleep(1)
            rospy.loginfo("Serial connection established on the port "+str(self.PORT))
        except:
            rospy.logwarn("Serial connection failure")

        self.pub_comm      = rospy.Publisher('/zumo32u4/command', String, queue_size=10)
        rospy.loginfo("Publisher initialization success /zumo32u4/command")
        self.pub_imu       = rospy.Publisher('/zumo32u4/imu', Imu, queue_size=10)
        rospy.loginfo("Publisher initialization success /zumo32u4/imu")
        self.pub_odom      = rospy.Publisher('/zumo32u4/odom', Odometry, queue_size=10)
        rospy.loginfo("Publisher initialization success /zumo32u4/odom")
        self.sub_sensorval = rospy.Subscriber('/zumo32u4/sensorval', String, self.subsensorval)
        rospy.loginfo("Subscriber initialization success /zumo32u4/sensorval")
        self.tf_br         = tf.TransformBroadcaster()
        rospy.loginfo("TransformBroadcaster initialization success")

    def __delete__(self):
        self.ser.close()

    def pubcommand(self):
        try:
            self.ser.flush()
            command = ""
            command = self.ser.read()
            if command != "":
                pub.publish(command)
        except:
            #print "pubcommand Error"
            #traceback.print_exc()
            pass

    def subsensorval(self, svalue):
        try:
            if len(svalue.data) > 0:
                self.sensorvalue = svalue.data.split(',')
                if len(self.sensorvalue) == 14:
                    self.pubimu()
                    self.pubodom()
        except:
            print "subsensorval Error"
            traceback.print_exc()
            #pass

    def pubimu(self):
        self.p.linear_acceleration.x=4*9.81*(float(self.sensorvalue[1])/2**16)/100
        self.p.linear_acceleration.y=4*9.81*(float(self.sensorvalue[2])/2**16)/100
        self.p.linear_acceleration.z=4*9.81*(float(self.sensorvalue[3])/2**16)/100
        self.p.orientation.x= float(self.sensorvalue[4])
        self.p.orientation.y=float(self.sensorvalue[5])
        self.p.orientation.z=float(self.sensorvalue[6])
        self.p.header.stamp = rospy.Time.now()
        self.pub_imu.publish(self.p)

    def pubodom(self):
        if self.sensorvalue[10]!=self.odomR or self.sensorvalue[9]!=self.odomL:
            deltat=(float(self.sensorvalue[0])-float(self.temps))/1000                        #Second
            VR=(float(self.sensorvalue[10])-self.odomR)/self.COUNT *3.14*self.DIAMETER/deltat #Meter
            VL=(float(self.sensorvalue[9])-self.odomL)/self.COUNT *3.14*self.DIAMETER/deltat  #Meter
            self.odomL=float(self.sensorvalue[9]) 
            self.odomR=float(self.sensorvalue[10])
            self.temps=self.sensorvalue[0]
        else :
            VR=0
            VL=0        

        self.o.pose.pose.position.x += deltat*(VR+VL)/2*cos(self.theta)
        self.o.pose.pose.position.y += deltat*(VR+VL)/2*sin(self.theta)
        self.theta += deltat*(VL-VR)/self.INTERAXIS    

        quat = tf.transformations.quaternion_from_euler(0,0,self.theta)

        self.o.pose.pose.orientation.x = quat[0]
        self.o.pose.pose.orientation.y = quat[1]
        self.o.pose.pose.orientation.z = quat[2]
        self.o.pose.pose.orientation.w = quat[3]
        self.o.twist.twist.linear.x =(VR+VL)/2*cos(self.theta)
        self.o.twist.twist.linear.y =(VR+VL)/2*sin(self.theta)
        self.o.twist.twist.angular.z = (VL-VR)/self.INTERAXIS    
        self.o.header.stamp = rospy.Time.now()
        self.pub_odom.publish(self.o)

        pos = (self.o.pose.pose.position.x,
               self.o.pose.pose.position.y,
               self.o.pose.pose.position.z)

        ori = (self.o.pose.pose.orientation.x,
               self.o.pose.pose.orientation.y,
               self.o.pose.pose.orientation.z,
               self.o.pose.pose.orientation.w)       
        self.tf_br.sendTransform(pos, ori, rospy.Time.now(), 'base_link', 'map')

if __name__=="__main__":

    rospy.init_node("zumo", anonymous = True)
    zumo = Zumo()

    while not rospy.is_shutdown():
        zumo.pubcommand()
        sleep(0.001)

    rospy.delete_param("BLUETOOTH_PORT")
    rospy.delete_param("BLUETOOTH_BAUDRATE")
    zumo.ser.close()

最後にUbuntuからmicroUSBケーブルを外し、RaspberryPiとArduinoを接続する。

* RaspberryPi + Arduino + RPLidar A1M8 による地図生成の実行と可視化

では、一気通貫で地図生成と可視化を行ってみる。
RaspberryPiでMasterを起動する。

RaspberryPiでMasterを起動
$ roscore

masked.png

rosserialを実行する。

rosserialの実行
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

63.png

RaspberryPiでPythonプログラムを起動する。

RaspberryPiでPythonプログラムを起動
$ rosrun zumo32u4 zumo32u4.py

124.png

RaspberryPiでRPLidar A1M8連携用ROSノードを起動する。

RaspberryPiでRPLidarA1M8連携用ROSノードを起動
$ roslaunch rplidar_ros rplidar.launch

125.png

RaspberryPiでgmappingを起動する。

RaspberryPiでgmappingを起動
$ roslaunch rplidar_ros slam.launch

masked.png

RaspberryPiでstatic_transformation_publisherを起動する。

RaspberryPiでstatic_transformation_publisherを起動
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link laser 100
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 odom base_link 100

UbuntuでRaspberryPiのMasterを参照するように設定したうえで、rvizを起動する。

Ubuntuでrvizを起動
$ export ROS_MASTER_URI=http://raspberrypi.local:11311/
$ roscd zumo32u4
$ roslaunch zumo32u4 zumo32u4rviz.launch

ん? ま、まさか・・・
ついに RaspberryPi で動かせた〜!!!! :relaxed:

◆ はじめに へ掲載した画像のとおりとなった。
127.png
懸念していた Segmentation Fault も発生せず、問題なく動いているようだ。
時間を掛け過ぎたので、自動航行の実装やチューニングはまたの機会に行うことにする。

◆ おまけ

1.ログ出力レベルの変更
下記で、ノード実行途中でもオンラインで出力レベルを変更できる。

ログ出力レベル変更(例)
$ rosservice call /<ノード名>/set_logger_level ros.<パッケージ名> DEBUG
slam_gmappingのログ出力レベル変更(例)
$ rosservice call /slam_gmapping/set_logger_level ros.gmapping DEBUG
ログレベル変更後の出力例
[DEBUG] [1526083622.733093545]: MessageFilter [target=odom ]: Removed oldest message because buffer is full, count now 5 (frame_id=laser, stamp=1526083621.842571)
[DEBUG] [1526083622.733609376]: MessageFilter [target=odom ]: Added message in frame laser at time 1526083622.578, count now 5
[ WARN] [1526083628.654677619]: MessageFilter [target=odom ]: Dropped 100.00% of messages so far. Please turn the [ros.gmapping.message_notifier] rosconsole logger to DEBUG for more information.

2.ノード・トピック・TFの関連性表示
PDFを出力してくれたり、現状の問題点を機械的に分析してくれる。
非常に助かる。

ノード・トピック・TF関連性表示コマンド
$ rosrun tf view_frames
$ roswtf

3.50回ぐらい目にして病みかけたエラーメッセージ例。もうお腹いっぱい。見たくない。。。

ErrorMessage
navigation/amcl/CMakeFiles/amcl.dir/build.make:62: ターゲット 'navigation/amcl/CMakeFiles/amcl.dir/src/amcl_node.cpp.o' のレシピで失敗しました
make[2]: *** [navigation/amcl/CMakeFiles/amcl.dir/src/amcl_node.cpp.o] エラー 1
CMakeFiles/Makefile2:22368: ターゲット 'navigation/amcl/CMakeFiles/amcl.dir/all' のレシピで失敗しました
make[1]: *** [navigation/amcl/CMakeFiles/amcl.dir/all] エラー 2
make[1]: *** 未完了のジョブを待っています....
[ 75%] Linking CXX executable /home/pi/catkin_ws/devel/lib/tf/testBroadcaster
[ 75%] Built target testBroadcaster
[ 77%] Linking CXX executable /home/pi/catkin_ws/devel/lib/tf/testListener
[ 77%] Built target testListener
[ 78%] Linking CXX shared library /home/pi/catkin_ws/devel/lib/liblaser_geometry.so
[ 78%] Built target laser_geometry
Makefile:138: ターゲット 'all' のレシピで失敗しました
make: *** [all] エラー 2
Invoking "make -j4 -l4" failed
pi@raspberrypi:~/catkin_ws $ 

◆ 本日のまとめ

  • RaspberryPi + Raspbian Stretch + GMapping による地図生成に成功した。
  • あまりにもトラブルシュートをやり過ぎたためか、段々とココにこういうものを置くとこういう動きをするな、とか、あれが足りていないな、とか、あまり深く考えなくても手が動くようになってきた。初学者にトラブルシュートは非常に良い学習機会なのかな、と思う。
  • 初心者が非公式な環境を構築するのは茨の道。
  • microSDカードは32GB
  • 大型のパッケージをビルドするときは外付けHDDをマウントしておかないとmicroSDカードがすぐに壊れそう。
  • TF、超難しい。 というか、しっかりとフレーム間の関係性を整理してから取り組まないと、ややこしすぎて混乱しまくってドンドン深みにハマっていく。
  • フレーム間のつなぎなどはまだまだ雑なので、追って整理していこうと思う。
  • Githubにコミットしてあるリソース一式は継続的にメンテナンスしていこうと思う。
  • あつかましいコメントだが、ビギナーの方に勇気を与えることができれば嬉しい。
  • Githubのソース(zumo32u4)

◆ 次回予告

Google Cartographer に挑戦したいと思う。
今回の作業が予想以上に手こずってトラウマになりかけたので、嫌な予感しかしないけども。
オッサンの奮闘はさらに続く。。。

◆ 次回記事

RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_009日目_SLAM_Google Cartographer_LiDAR(A1M8) へ続く

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
What you can do with signing up
38