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

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

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

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

今回	他記事	アルゴリズム	特徴
★		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カメラ専用、検証対象外とする

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

東北大学渡辺助教　Live USBと同等の環境構築方法(参考) → ros-kinetic-navigation と統合開発環境QtCreator の導入方法を参考
東北大学渡辺助教　ROSプログラミングの基本とロボット動作
東北大学渡辺助教　ROSを用いたURGのデータ取得
東北大学渡辺助教　ROSの便利機能
千葉工業大学原主任研究員　ROSを用いた自律移動ロボットのシステム構築
東北大学渡辺助教　ROS navigationパッケージを使ってみよう

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

SLAM入門: ロボットの自己位置推定と地図構築の技術

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

Navigation Stack を理解する - 1. 導入　Qiita

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

ROS(Robot Operating System)を使う　ログ表示　安曇野の森から

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

devel/setup.bashの自動実行設定

$ nano ~/.bashrc
下記を追記
source /home/<username>/catkin_ws/devel/setup.bash

#◆ この記事のおおまかな流れ
大きく下記の流れで環境構築と試行を行いながら進めていく。

検証用環境整備（Ubuntu16.04上でのLidar利用準備と測域のシミュレーション）
検証環境上でのGMapping（Ubuntu16.04上でのGMappingシミュレーション）
ロボット実機での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版) は導入済み のため、検証が終わり次第そちらへ環境を移行する。

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

##● 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する。

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

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

終わり。

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

##● 格安LiDAR RPLidar A1M8 のキッティング
< RPLidar A1M8の外観 >

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

・・・高いわ！！！

RaspberryPi3とほぼ同じ大きさ。

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

URG-04LX-UG01　北陽電機株式会社

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

無回転LiDAR Velarray LiDAR　Velodyne社

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

たった4000円で自動運転技術の肝となる「LIDAR」を自作　GIgazine

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

公式のGithubリポジトリ上のSDKバージョンはVer1.5.7。
PullRequestが６件近く半年以上も無視されていて、アップデートが止まっているようだったので、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

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"

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

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

rvizデモプログラムの起動

$ roslaunch rplidar_ros view_rplidar.launch

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

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

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

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

Masterの起動

$ roslaunch rplidar_ros rplidar.launch

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

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

Subscriberの起動

$ rosrun rplidar_ros rplidarNodeClient

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

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

TOPICの一覧表示

$ rostopic list

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

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

TOPICの内容表示

$ rostopic echo /scan

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

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

##● 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) あたりが重要なのかな。
ここで、先ほどまで意味不明だった数字の羅列の意味がようやく分かってきた。

色々調べているうちにモーターの動作スピードを調整できるらしいことが分かったので、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周波数に設定されているのかを調べる。
３０分後、見つけた。

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 かな。
すぐ１行上に 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可視化

###＊ 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

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

$ rosrun map_server map_saver -f map

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

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

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

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

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

参考記事(必要であればturtlebot-gazebo導入後に実施)　After install ROS Kinetic, cannot import OpenCV

自分は今まで散々環境構築に時間を掛けてきてハマりちらかしてきただけあって、自力でなんとかできる知識レベルにありそうだったので、そのまま 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

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

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

turtlebotをキーボードで動かすノードを起動

$ roslaunch turtlebot_teleop keyboard_teleop.launch

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

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

rosbagによるシミュレーション記録データの生成

$ rosbag record -O test.bag /scan /tf

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

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

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

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

シミュレーション記録データの情報参照用コマンド

$ rosbag info test.bag

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

Masterの起動

$ roscore

別のターミナル上でslam_gmappingを起動

$ rosparam set use_sim_time true
$ rosrun gmapping slam_gmapping

別のターミナル上でtest.bagをプレイバック

$ rosbag play --clock test.bag
３分間ものbagデータを記録してしまったため、カップラーメンに湯を足しながら３分間のプレイバック完了を待つ

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

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

地図を生成するコマンド

$ rosrun map_server map_saver -f map2
$ rosparam set use_sim_time false

一瞬で終わった。

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

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

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

##● 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による、ロボットへの前後進、右左折、停止、指示操作

まず、作業用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

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

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

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

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

大量にあるパッケージ間の依存関係の解消作業
コンパイラバージョンとパッケージバージョン間の整合作業
CMAKE_PREFIX_PATHの不整合の特定と解消
最大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キーを押す

下記のように設定。

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軸磁力計、３軸ジャイロ計を同梱) を使用し、加速度と方角を取得するプログラムを作成する。

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

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

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

表示されたエラーメッセージをキーワードにググってみると下記の記事を見つけた。
今回作成したプログラムは、１Publish あたり 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

いくつか候補が表示されているが、とりあえず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

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

TOPICの確認

$ rostopic list

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

ArduinoからPublishされたsensorvalトピックの確認

$ rostopic echo /zumo32u4/sensorval

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

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

$ rosrun zumo32u4 zumo32u4.py

エラーにならずに動き始めたようだ。
遠隔操作用の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へスケッチを書き込む。

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

rosserialを実行する。

rosserialの実行

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

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

RaspberryPiでPythonプログラムを起動

$ rosrun zumo32u4 zumo32u4.py

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

RaspberryPiでRPLidarA1M8連携用ROSノードを起動

$ roslaunch rplidar_ros rplidar.launch

RaspberryPiでgmappingを起動する。

【参考記事】 KMi robots: making a map with a Roomba, an RPlidarA2 and gmapping

RaspberryPiでgmappingを起動

$ roslaunch rplidar_ros slam.launch

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 で動かせた〜！！！！　

[◆ はじめに](#◆ はじめに) へ掲載した画像のとおりとなった。

懸念していた Segmentation Fault も発生せず、問題なく動いているようだ。
時間を掛け過ぎたので、自動航行の実装やチューニングはまたの機会に行うことにする。

#◆ おまけ
１．ログ出力レベルの変更
下記で、ノード実行途中でもオンラインで出力レベルを変更できる。

ログ出力レベル変更（例）

$ 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.

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

ノード・トピック・TF関連性表示コマンド

$ rosrun tf view_frames
$ roswtf

３．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)　へ続く