Donkeycar の lidar 実装を読み解く

DonkeycarにLidarを積んでみたいのだが、すでに donkeycar.parts.lidar.py にパーツクラスが用意されている。

用意されているのだけど..

とりあえず Veicleへaddして動いているようにみえるのだけど..これであってるんだろうか..

ということでいつもどおりといえばいつもどおりなのだけど、 Donkeycar パッケージのソースコードをたどってみた（..でもコメント記述がぜんぜんないなあ..）。


donkeycar.parts.lidar.py の最初に RPLidar というパーツクラスが書かれているので動作しそうなレーザオブジェクトが Slamtec PRLidar であることはわかる。

run_threadedメソッドの引数がdistancesとanglesか..

両方floatの配列になっていて、角度がangles[0]のときの壁までの距離が distances[0]、以下順番に..になってるらしい。おそらく配列数は同じで、スキャンした際の件数でこの配列サイズもことなりそうだ..



LidarPlot もパーツクラスで、runメソッドの引数がdistancesとanglesで戻り値がframe、コンストラクタの引数 resolutionで指定した３色Imageオブジェクト..

つまり、点群データをもとにImageオブジェクト化したものを出力するパーツか..


で、BreezySLAMクラスは..runメソッドの引数がdistancesとanglesに加えて、map_bytesか..引数は x, y, theta..ってことは平面座標系の(x, y)とDonkeycarの向きってことか..

SLAMのアルゴリズムはDonkeycarパッケージ本体には実装がなく、https://github.com/simondlevy/BreezySLAM のRMHC_SLAM クラスを使っているらしい..

つまり、Donkeycarが提供するSLAMを使うには、まず BreezySLAMがどんなRMHC(Random-Mutation Hill-Climbing)探索アルゴリズムになっているか知らないといけないわけだ。

それはおいといて、先に次のパーツクラス BreezyMap は..初期化した配列を渡しているだけ？！

最後のMapToImgeは..バイト配列をnd.array化してるだけか..たぶんカメライメージのかわりにこの出力イメージでDonkeycarを動かせってってことか..


BreezySLAM https://github.com/simondlevy/BreezySLAM の README.md もとりあえず翻訳してみたのが以下。

他の記事同様 at your own risk で参照のこと。

----
BreezySLAM

このリポジトリ https://github.com/simondlevy/BreezySLAM には 、Python で Lidar http://en.wikipedia.org/wiki/Lidar ベースのSLAM http://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping の使用を開始するために必要なすべてのものが含まれています 。（Matlab、C ++、およびJavaのサポートもありますが、この種の作業ではPythonを使用する場合が多いため、これらの言語のコードを更新しなくなりました。）BreezySLAMはLinuxおよびMac OS X上のPython 3、LinuxおよびWindows上のC++で動作します。Python C 拡張を使用することにより、PythonおよびMatlabのバージョンをC ++と同じくらい高速に実行することができます。32ビットプラットフォームで最大の効率を実現するために、コードの計算集約的な部分でストリーミングSIMD 拡張機能（Intel）および NEON（ARMv7）を使用しています。

BreezySLAMは 、同僚の Ken Lambert http://home.wlu.edu/%7Elambertk/ が開発したグラフィカルユーザーインターフェイスへの Breezy http://home.wlu.edu/%7Elambertk/#Software アプローチ に触発されました。右上の画像は、BreezySLAM ユーザ https://www.linkedin.com/pulse/slam-your-robot-drone-python-150-lidar-chris-fotache によって作成された家のフロア全体のマッピングのサンプルです。

以下のコードサンプルで示されたとおり（Lidarパラメータ、マップサイズ（ピクセル）およびマッピング領域（メートル）を引数とするコンストラクタ、現時点のスキャンをもとに更新を行う `update` メソッド、現在のロボット位置を返す `getpos` メソッド、現在のマップをバイト配列として取得する `getmap` メソッド）、基本的なAPIは非常に単純です。

from breezyslam.algorithms import RMHC_SLAM
lidar = MyLidarModel()
mapbytes = bytearray(800*800)
slam = RMHC_SLAM(lidar, 800, 35)

while True:
    scan = readLidar()
    slam.update(scan)
    x, y, theta = slam.getpos(scan)
    slam.getmap(mapbytes)

オドメトリが利用可能な場合は、update メソッドに渡すこともできます。

Python環境へのインストール

BreezySLAMのインストールでは、一般的な distutils http://docs.python.org/2/distutils/introduction.html アプローチを使用してPythonパッケージをインストールするため、ファイルをダウンロードして解凍し、BreezySLAM/python に  cd し、以下のコマンドを実行します。

sudo python3 setup.py install

簡単なデモを見るには、 BreezySLAM/examples へ cd して、以下のコマンドを実行します。

make pytest

実行すると、ログファイル exp2.dat のLidarスキャンおよび走行距離データのマップとロボットの軌跡を示すPGMファイルが生成・表示されます 。Pythonイメージングライブラリ http://www.pythonware.com/products/pil/ がインストールされている場合は、代わりにPNGファイルを生成するスクリプト log2png.py を試すことができます。

PyRoboViz https://github.com/simondlevy/PyRoboViz がインストールされている場合、次のコマンドによりライブアニメーションを表示する事ができます。

make movie

Makefile の上部にあるパラメータ USE_ODOMETRY を 0（ゼロ）に設定することにより、走行距離測定をオフにできます。パラメータ RANDOM_SEED をコメントアウトすることにより、パーティクルフィルタ（モンテカルロ位置推定）をオフにできます。

その他の機能については、以下のコマンドを実行して確認してください。

pydoc3 breezyslam

コンポーネントクラス Map 、Scan、および Position 、そしてdistanceScanToMap() メソッドを使用することにより、独自の新しいアルゴリズムとパーティクルフィルタを開発できます。

北陽 URG04LX によるテスト

Linuxの場合、 BreezyLidar http://home.wlu.edu/~levys/software/breezylidar/ パッケージ、OpenCV Pythonパッケージをインストールし、examples フォルダーでサンプルコード urgslam.py を試して ください。

GetSurreal XV Lidar によるテスト

BreezySLAMには、GetSurrealの安価な XV Lidar https://www.getsurreal.com/product/xv-lidar-sensor-mount-package の Pythonサポートが含まれています。試行には xvlidar https://github.com/simondlevy/xvlidar Pythonパッケージも必要です。両方のパッケージをインストールしたら、 BreezySLAM/examples フォルダで xvslam.py サンプルを実行できます。

SLAMTEC RPLidar A1 によるテスト

BreezySLAMには、SLAMTECHの安価なRPLidar A1 http://www.slamtec.com/en/lidar/a1 の Pythonサポートも含まれています。試行には rplidar https://github.com/SkoltechRobotics/rplidar Pythonパッケージも必要です。パッケージをインストールしたら、 BreezySLAM/examples フォルダで rpslam.py サンプルを実行できます。

Matlab 用インストール

64ビットWindows、Linux、Mac OS X上のMatlabでBreezySLAMが実行できます。matlab のディレクトリには3つのすべてのオペレーティングシステム用のコンパイル済みのバイナリを含め、必要なすべてのコードが含まれています。Matlabでの試行には、このディレクトリをパスに追加し、 examples ディレクトリに移動して、以下のように実行してください。

  >> logdemo('exp2', 1)

ソースコード修正や別のOSでの実行のためのビルドを行う場合、 matlab ディレクトリへ移動し、以下のように実行してください。

  >> make

Windows用バイナリを作成する際にトラブルに遭遇した場合、 これらの手順 http://www.mathworks.com/matlabcentral/answers/95039-why-does-the-sdk-7-1-installation-fail-with-an-installation-failed-message-on-my-windows-system が役に立ちます。

C++ 用インストール

BreezySLAM/cpp へ cd して、以下のように実行してください。

sudo make install

これにより、 libbreezyslam シェアードライブラリが /usr/local/lib ディレクトリにデプロイされます。シェアードライブラリを他の場所に保持する場合 は、 Makefile の上部にある変数 LIBDIR を変更するだけです。~/.bashrc ファイルに次の行を追加する必要がある場合もあります。

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib

簡単なデモを見るには、 BreezySLAM/examples に cd し、以下のように実行してください。

make cpptest

繰り返しになりますが、 /usr/local/lib を使用しない場合は、 Makefile の上部にある変数 LIBDIR を変更する必要があります。

Java用インストール

BreezySLAM/java/edu/wlu/cs/levy/breezyslam/algorithms および BreezySLAM/java/edu/wlu/cs/levy/breezyslam/components にある Makefile の変数 JDKINC にJDKインストール先を記述してくさだい。そしてそれらのディレクトリ内で make を実行してください。

簡単なデモを見るには、 BreezySLAM/examples に cd し、以下のように実行してください。

make javatest

Windows用インストール時の注意

私や他の方々がWindows上でのインストールに苦労 http://stackoverflow.com/questions/2817869/error-unable-to-find-vcvarsall-bat したため、Windows上のPythonバージョンはサポートしなくなりました。もし試したい場合は、 ここ https://docs.python.org/2/extending/windows.html を参考にしてください。

Windows上のC++ライブラリを使ったビルドの際には、MinGWを使用しました。使用するC++コンパイラが何であれ、.dllファイルの場所を環境変数 PATH に追加する必要があります。

新規パーティクルフィルタの追加

BreezySLAMはCoreSLAM（ tinySLAM https://openslam.org/tinyslam.html ）コードベースで構築されているため、SLAMのマップ構築コンポーネントとパーティクルフィルタリング（モンテカルロ位置推定）コンポーネントを明確に分離しています。新規のパーティクルフィルターを追加するには、 breezyslam.algorithms.CoreSLAM クラスまたは  breezyslam.algorithms.SinglePositionSLAM クラスをサブクラス化し、関連メソッドを実装します。

著作権、ライセンス、質問

著作権やライセンス( Gnu LGPL https://www.gnu.org/licenses/lgpl.html ) に関する情報は、各ソースコードのヘッダに記載されています。

関係者

Suraj Bajracharya、Simon D. Levy、Matt Lubas、Alfredo Rwagaju

謝辞

この研究は、革新技術センタ（CRCF＃MF14F-011-MS）からのCommonwealth Research Commercialization Fund助成金によって一部サポートされていました。このパッケージのデバッグとテストに協力してくれたOlin CollegeのMichael Searingに感謝いたします。
-----

ここでわかった重要なことは、動きそうなレータが北陽  URG04LX と RPLidar A1、そしてGetSurreal XV Lidar。
GetSurreal XV Lidarは実際どれが本体なのかわからないけど、まえの２つはてにはいりそうだ。

BreezySLAMのレーダ実装のちがいはどこで確定させているかをみるには breezyslam.algorithms.py を読めばいいってことらしい..

で、 breezyslam.algorithms.py のクラスと主要なメソッドの機能コメントを翻訳したのが、以下である。

----
breezyslam.algorithms パッケージ

CoreSLAM クラス

CoreSLAM クラスは、Position、Map、Scan、およびLaserクラスを使用する抽象クラス。

単純なCoreSLAM（tinySLAM）アルゴリズムの多様な具現実装を実行するために使用する。

• 著者：Bruno Steux 、 Oussama El Hamzaoui
• タイトル：CoreSLAM: Cコード200行以下のSLAMアルゴリズム
• 書籍タイトル：11th International Conference on Control, Automation, Robotics and Vision, ICARCV 2010, Singapore, 7-10 December 2010, Proceedings
• ページ：1975-1979
• 出版元：IEEE
• 年：2010

具現クラスでは、点群（パーティクルクラウド）とマップ（should_update_map が trueの場合）を更新するメソッド _updateMapAndPointcloud(scan_mm, dxy_mm, dtheta_degrees, should_update_map) の実装が必要となる。

メソッド init(self, laser, map_size_pixels, map_size_meters, map_quality=_DEFAULT_MAP_QUALITY , hole_width_mm=_DEFAULT_HOLE_WIDTH_MM)

新しいLidarおよび走行距離データでの更新に適したCoreSLAMオブジェクトを作成する。

• laser : Lidarユニットの仕様を表すレーザーオブジェクト
• map_size_pixels :ピクセル単位の正方形マップのサイズ
• map_size_meters :メートル単位の正方形マップのサイズ
• map_quality : マップへのスキャンの統合速度を0〜255の値で指定
• hole_width_mm : 障害物（壁）の幅を決定する

メソッド update(self, scans_mm, pose_change, scan_angles_degrees=None, should_update_map=True)

スキャン情報と走行距離を更新し、具現クラス側で独自実装された _updateMapAndPointcloud メソッドを呼び出して独自変更処理を行う。

• scan_mm : Lidarスキャン値のリスト、そのカウントはCoreSlamコンストラクタに渡されるLaserオブジェクトの属性 scan_size で指定される
• pose_change : オドメトリから計算されたタプル (dxy_mm, dtheta_degrees, dt_seconds)
• scan_angles_degrees : scans_mm の距離に対応する角度のオプションリスト
• should_update_map :マップを更新するかどうか（真偽値）

メソッド getmap(self, mapbytes)

bytearray(バイト配列) mapbytes を現在のマップピクセルで埋める。bytearray 長は、CoreSLAM .__ init __() に渡される引数 map_size_pixels の2乗となる。

• mapbytes : 現在のマップピクセル(バイト配列)

メソッド setmap(self, mapbytes)

現在のマップピクセルを引数で指定したbytearray（バイト配列）の値に設定する。bytearray長は、 CoreSLAM .__ init __() に渡される引数 map_size_pixels の2乗となる

• mapbytes : セットするマップピクセル(バイト配列)

クラス SinglePositionSLAM(CoreSLAM)

SinglePositionSLAMは、単一のポイント（位置）を持つポイントクラウドを使用して CoreSLAM を実装する抽象クラス。 具現クラスでは、_getNewPosition(self、start_position) メソッドを提供して、開始位置からの検索に基づいて新しい位置を計算する実装を行う必要がある。

メソッド _updateMapAndPointcloud(self, dxy_mm, dtheta_degrees, should_update_map)

マップとポイントクラウド（パーティクルクラウド）を更新する。 CoreSLAM.update() によって自動的に呼び出される。

速度は、タプル (dxy_mm, dtheta_degrees, dt_seconds) 形式（引数として指定する）。

メソッド getpos(self)

現在の位置をタプル(x_mm, y_mm, theta_degrees) として返却する。

クラス RMHC_SLAM(SinglePositionSLAM)

クラス RMHC_SLAM は、ランダム突然変異ヒルクライミング(RMHC:Random-Mutation Hill-Climbing)検索を使った、(親でもある)抽象クラスSinglePositionSLAMの_getNewPosition() メソッド実装を提供する。なお効率化のために、独自の内部擬似乱数ジェネレータを使用している。

メソッド init(self, laser, map_size_pixels, map_size_meters, map_quality=_DEFAULT_MAP_QUALITY, hole_width_mm=_DEFAULT_HOLE_WIDTH_MM, random_seed=None, sigma_xy_mm=_DEFAULT_SIGMA_XY_MM, sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES, max_search_iter=_DEFAULT_MAX_SEARCH_ITER)

新しい Lidar および走行距離データによる更新に適した RMHCSlam オブジェクトを作成します。

• laser : Lidarの仕様を表す Lidar オブジェクト
• map_size_pixels :ピクセル単位の正方形マップのサイズ
• map_size_meters :正方形マップのサイズ（メートル単位）
• map_quality : 0〜255の値を指定して、マップへのスキャンの統合速度を決める
• hole_width_mm :障害物（壁）の幅を決定する
• random_seed :再現可能な結果をサポートするための引数。 指定されていない場合のデフォルトはシステム時間となる
• sigma_xy_mm :RMHC検索の位置の (X, Y) コンポーネントの正規分布の標準偏差をミリメートル単位で指定
• sigma_theta_degrees :RMHC検索の位置の回転成分の正規分布の標準偏差を度数で指定する
• max_search_iter :RMHC検索の最大反復回数を指定

メソッド _getNewPosition(self, start_position)

親クラス SinglePositionSLAM が使用する _getNewPosition() メソッドの実装を提供する。 ランダム突然変異ヒルクライミング(RMHC)検索を使用して、開始位置に基づいてより適切な位置を探索する。

クラス Deterministic_SLAM(SinglePositionSLAM)

クラス SinglePositionSLAMが使用する _getNewPosition() メソッドを、検索開始位置をコピーするだけの実装で提供する具現クラス。

メソッド init(self, laser, map_size_pixels, map_size_meters, map_quality=_DEFAULT_MAP_QUALITY, hole_width_mm=_DEFAULT_HOLE_WIDTH_MM)

新しいLidarおよび走行距離データでの更新に適した Deterministic_Slamオブジェクトを作成する。

• laser :レーザ仕様を表す Lidar オブジェクト
• map_size_pixels :ピクセル単位の正方形マップのサイズ
• map_size_meters :正方形マップのサイズ（メートル単位）
• map_quality : 0〜255の値で、マップへのスキャンの統合速度を指定
• hole_width_mm :障害物（壁）の幅を決める

メソッド _getNewPosition(self, start_position)

開始位置のコピーを返却する。
----

DonkeycarのRPLidarクラスをそのままつかえばA1M8が動きそうだな..
Amazonで調べると去年５万円弱だったのが１万円台にまで下がってる..

..ちょっと、ためしてみるか..