Jetson Nano 上で RealSense T265 用 Donkeycar パーツを試す

Donekycar (v3.1.1マスタブランチ)コードには
RealSense T265用パーツクラス
（donkeycar.parts.realsense2.pyのRS_T265という名前のクラス）
がすでに用意されている。

realsense2.py は、pythonパッケージ pyrealsense2 を先に導入しておかなくてはならない。

通常のUbuntuなら pip install pyrealsense2 でインストールするのだが、Jetson Nano用のJetpack (v43)アーキテクチャ用whlモジュールがPyPIに登録されていないので、シェアードライブラリ含めてソースコードビルドを選択せざるを得ない。

Donkeycar環境を構築済みのJetson Nanoへ

GitHub: IntelRealSense/librealsense
https://github.com/IntelRealSense/librealsense

上記のリポジトリからソースコードビルドした。
＃ビルド方法の概要は前記事のps以下を参照のこと

python -c "import pyrealsense2" を実行してエラーが出なければ、python ラッパパッケージ pyrealsense2 のインストールが成功しているかがわかる。

また、librealsense/wrapper/python/examplesにある t265_example.py を実行(JetsonにUSBケーブル経由でT265をつないで起動し、python t265_example.pyを実行)することで、実際に値が取れているかどうか疎通確認テストができる。

以下のコードは、Veichleフレームワークを使ってRS_T265パーツを動かすサンプルである。

    import donkeycar as dk
    V = dk.vehicle.Vehicle()

    from donkeycar.parts.realsense2 import RS_T265
    rs = RS_T265(image_output=False)
    V.add(rs, outputs=['pos', 'vel', 'acc', 'image_array'], threaded=False)
    class PrintIMU:
        def run(self, pos, vel, acc):
            print('pos:({:.5g}, {:.5g}, {:.5g}) vel:({:.5g}, {:.5g}, {:.5g}) acc:({:.5g}, {:.5g}, {:.5g})'.format(
                pos.x, pos.y, pos.z, vel.x, vel.y, vel.z, acc.x, acc.y, acc.z))
    V.add(PrintIMU(), inputs=['pos', 'vel', 'acc'])

    V.start(rate_hz=20, max_loop_count=20 * 60)

RS_T265 を run すると6軸IMUデータ＋位置座標（位置座標(X, Y, Z)、速度(X, Y, Z)、加速度(X, Y, Z)）とイメージが取得できる。

ただし、RS_T265クラスのコンストラクタ引数に何も指定しない場合（デフォルトの場合）は、イメージ配列が出力されない(Noneで出力される)。

T265カメラの画像のイメージ配列を取得したい場合は、RS_T265(image_output=True)と指定するとnd.array形式の配列として出力される。

ただし、2つあるカメラの片方（多分..左側かな..）のみでかつnd.array形式のshapeが(800, 848)となる（白黒画像なので2次元配列）。

Donkeycar のカメラの代替として使用する場合は、myconfig.pyのIMAGE_H、IMAGE_W、IMAGE_DEPTHを書き換えるか、イメージ配列を加工して(120, 160, 3)にreshapeするパーツクラスを作成すれば良い。


なお pyrealsense2 パッケージを直接使うと、四元数(qw, qx, qy, qz)が取得できる。
詳細は librealsense/wrapper/python/examples/t265_rpy.py を参照のこと。


RS_T265クラスのコンストラクタのデフォルト引数がイメージを使用しない仕様から、おそらくカメラは既存のデバイスを使って、IMU(MPU6050)がわりにT265を使い、機械学習モデル'imu'で動かすことを想定して作成されたものと推測できる。

それならMPU6050のほうが安上がりだ。位置情報もSLAMを使うにはDonkeycarの機械学習モデル自体の入力層をいじらなくてはならないし..


..やはりROSで使うのが、一般的なのかな..
シェアードライブラリのみ(Pythonラッパなし)や
ROS上で動作する T265モジュール(melodic)も
apt で入れられるし..