depthai-core examples を試す。
depthai-core/examples に、プログラムサンプルが色々有るので、ビルドして試してみました。
1. 先に、depthai-ros のインストールが必要です。
depthai-ros のインストールは、Jetson Nano 2G ROS Oak-D-Lite を参照して下さい。
2. depthai-ros のインストール後に、Examples をビルドします。
ビルド方法は、luxonis/depthai-core の Running examples にあります。
$ cd ~/local/oakd-lite/depthai-core
$ rm -rf build
$ mkdir build
$ cd build
$ cmake .. -D'DEPTHAI_BUILD_EXAMPLES=ON'
$ cmake --build .
一応簡単にビルド出来ました。
build/examples に、作成されます。
3. Jeston Nano 2G の rtabmap_ros で、将来使えそうな、
StereoDepth/rgb_depth_aligned.cpp を試してみました。
これを参考にして、 ROS で、depth データと、中央の RGB カメラの rgb データを、同期してpublish するnodelet を作れば、
rtabmap_ros でそのまま使えそうです。
fps も設定できる様です。
fps=15 位で、軽量に実現できれば、 Jeston Nano 2G and rtabmap_ros で使えそうです。
注1) fps の設定は、今は、未対応みたいです。自分でプログラムで押さえないといけないみたいです。
デフォルトだと、30[Hz] になります。
注2) RGB 画像と、Depth 画像が完全に一致していません。Rtabmap_ros で使う時、この誤差がすこし問題になりそうです。
3. Ros での使用。
早速、Ros で使えるように、RGB 画像と、Depth を Publish する様に組み込んでました。
結局、下の 3 本を参考に、作る羽目になりました。
どれも、帯に短し、襷に長しで、大変でした。
depthai-core/examples/StereoDepth/rgb_depth_aligned.cpp
depthai-core/examples/MonoCamera/mono_preview.cpp
luxonis/depthai-ros-examples/depthai_examples/ros1_src/stereo_publisher.cpp
使ってみて、やはり OAK-D-Lite では、タイムラグがどうしてもあって、
自作 Trurtelbot3(foxbot_core3) and rtabmap_ros では、あまり良い性能は出ないようです。
おまけに、rtabmap_ros/point_cloud_xyz とはデータの不一致で連携できません。
注) これは、 rtabmap_ros/point_cloud_xyz のパラメータを変えたら、OK になりました。
後日、github にでも公開したいですが、SSL 対応の方法が判らないので.....
しばらくお待ちください。
4. github に公開しました。
github.com/tosa-no-onchan/depthai_ros_app
rgb_depth.cpp
1) 環境
JetsonNano 2G
ROS: melodic
OpenCV4.x
2) ビルドは、
$ catkin_make --pkg depthai_ros_app -j1
3) 実行方法は、
$ rosrun depthai_ros_app rgb_depth
または、
$ roslaunch depthai_ros_app rgb_depth.launch
5. rtabmap_ros with rgb_depth_aligned of OAK-D-Lite で試す。
自作 Turtlebot3(foxbot_core3) で、/camera/sync をサブクライブして、それに同期して、odometey と
tf_basefootprint をパブリッシュさせれば、
rtabmap_ros/rtabmap のパラメータ
approx_sync=true
で使えます。
これで、やっとなんとか使えそうな、3D-Map ができるようになりました。
後は、odom_off=0 から30 、rate=13 を色々変えて良い値を使うようにします。
下記に、rtabmap-nishi_depthai_test3.launch を公開します。
/free-soft/rtabmap_ros_my/launch/
後は、rtabmap_ros がナビゲーションモードのとき、下記ワーニングが出ているので、
GTSAM or g2o ライブラリーを入れなければいけないぞね!!
GTSAM が簡単そうなので、インストールしてみます。
/introlab/rtabmap/wiki/Installation
(Recommended) GTSAM:
パッケージ版があるとの事です。
gtsam.org/get_started/
Install GTSAM from Ubuntu PPA
GTSAM を入れて、rtabmap_ros のパラメータを、
Optimizer/Strategy="2" # 2=GTSAM
にたら、ナビゲーションモードでの動作が、ずいぶんスムーズになりました。
自作 Stereoカメラよりは、良いかもしれません。が、
未だ、 rtabmap_ros/point_cloud_xyz と連携が取れていないので、move_base での、障害物判定ができません。
注) これは、rtabmap_ros/point_cloud_xyz のパラメータを変えたら、OK になりました。
decimation=4 を止めて、デイフォルト値(=1) にしたら、OK になりました。
但し、これを move_base と連携させると、move_base がエラーを吐き出します。
Invalid Trajectory 0.000000, 0.000000, 0.100000, cost: -6.000000 が問題で、
How to avoid "Invalid Trajectory (cost: -1.000000)" in DWA Local Planner?
1 Answer
It means that it can't find any valid plan. Invalid plans have negative costs.
線路計画が立てられないとのことです。
Got new plan は、python プログラムから、Turtlebot3 へ回転のデータを送った事への返答です。
それに従って、線路計画を建てようとしたら、plan cost=-6.0 になった。
多分、rtabmap_ros/point_cloud_xy の出力の PointClouds2 のデータから、回りにいっぱい障害物があると判定しているのでは?
rtabmap_ros/point_cloud_xyz に与える、depth データにまだ、問題があるのだろう?
まだしばらくは、rtabmap_ros/point_cloud_xyz を使えないです。
Rviz で、rtabmap_ros/point_cloud_xyz の出力する、PointClouds2 と、rtabamap_ros が出力する、3D-map と重なるかチェックしないと行けないぞね!!
Rviz で確認すると、ピッタリと重なっています。文句のつけようがありません。
これがNG だとすると、床を障害物と判定しているのかも知れません?
costmap_common_params_waffle.yaml
min_obstacle_height: 0.05 -> 0.1 -> 0.2
にしてみます。
tf の値を、 OAK-D-Lite のカメラの取り付け位置の実際の高さにしないといけないか?。
下記では、19[cm] の高さだが?
もう少しだけ、スループットが上がれば、良いのだが?
いまは、残念としか言いようがない。
本当にもう少しだけ、タイムラグが無くなれば良いのだが。
根本的に、depthai_core の作りが問題だと思う。
depthai_core の方で自分の rate で勝手に Camera にキャプションして、Pipe に詰め込むので、どうしても、timestamp が古くなる。
こちらが欲しいのは、今 Camera でシャッターを押してキャプションしたそのデータが、今欲しいのであって、
古いデータはほしくない。
でないと、自作ロボットの odometry や、tf-base_footprint と一致しない画像を取り込むことになる。
今の方法では、どうしても、キャプションが、40[ms] から 60[ms] 程古い画像しか取れない。
この状態で自作ロボットを、python プログラムで、１回転させて、rtabmap_ros の 3D-map を、Rviz で見てみると、
部屋の壁が斜めに描画されてしまう。部屋の四隅の角が、なくなってしまいます。
これが、一番残念な所です。
どうやら、depthai-core/src/device/DataQueue.cpp のDataOutputQueue コンストラクターで
スレッドに移行して、その中で、
XLinkStream stream(*conn, name, 1);
の stream から、ドンドコ取り込んで、pipe へ書き込んでいるようです。
まるっきりの非同期 Read。なんで、こんな作りじゃ?
やはり、stream の送り側が、非同期でどんどこ送ってくるみたいです。
せめて、こちらが、caputure をcall したら、Camera からデータを取り込んで、間に変換用のフィルターをかます作りにしてほしかった。
pipe からの取り出しも、この同じクラスの中のメソッドで取り出しています。
試しに、自分で、直接 stream から取り込む様に改造もしてみましたが、それでも、タイムラグが、40[ms] から 42[ms] で取り組むのが限界でした。
ただし、この方法だと、何故か Publish の 速度がまるっきり出ませんでした。
もう一度オリジナルのプログラムに戻って、
30[Hz] で常にRGB、Depth データを取り込むと、タイムラグが、40[ms] から 46[ms] ほどになって、其れが最小です。
その中から、間引いて、15[Hz] でパブリッシュするようにしました。
これが、限界のようです。
やはり、rtabmap_ros で、 OAK-D-Lite は、あまりお勧めしません。!!
既に記述しましたが、RGB 画像と、Depth 画像が完全に一致していないので、性能がよくありません。
これから購入を考えている方は、他の Depth カメラをお勧めします。
おんちゃんは、せっかく買ったので、もう少し、使ってみます。.... これは、時間のむだ 止めました。
STMicroelectronics の VD55H1 と カメラと一体の商品がでたら、まっさきに購入していみたいぞね!!!。
You tube
やはり、Mouser 辺りで D435 を購入したほうが良いのかもしれないぞね!!
6. depthai_core のバグフィックスを見ていたら、(2022.3.22)
github.com/luxonis/depthai-core/releases
Release v2.15.0 で、幾つか改善されたみたいぞね。
Stereo
Depth and RGB alignment optimization
FPS change and image orientation control for IMX214 color camera (on OAK-1 Lite and OAK-D Lite)
但し、depth (Mono) の方の FPS 指定は、どうなのか?
最新版を試してみないといかんぞね!!
rgb_depth_aligned.cpp で早速チェックしてみました。(2022.3.23)
1) depth と rgb の重なりは、改善されたみたいです。
2) FPS の指定も、RGB カメラは、OK です。但し、やはり、Mono Camera(OV7251 なのなか?) は、未だみたいです。
早速、rtabmap_ros with depth camera で試してみたが、あまり変わらない。
やはり、タイムラグの多さが、致命的だ!!
rgb_depth.cpp の方もプログラムを見直したので、少しは、タイムラグが少なくなったようです。
RGB データで、35[ms] - 38[ms] のタイムラグにはなりました。
Depth の方は、Mono Camera の FPS が変えられないので、今いちです。
部屋の壁を斜めに捉えた時の、Depth データが精度が悪いような気がします。
こちらのプログラムが悪いのか、Depth データの方が悪いのかは、はっきりしませんが。
OAK-D-Lite は、rtabmap_ros では、使いものにならないぞね。
STマイクロエレクトロニクスの VD55H1 が、製品化されるのを待つしかない。