Jetson Nano 2G ROS rtabmap_ros with Oak-D-Lite
いよいよ本題の、rtabmap_ros で、Oak-D-Lite(depthai) を使って見ることにします。
自作 Turtlebot3(foxbot_core3) に組み込んで、確認してみます。
1. 環境:
自作 Turtlebot3(foxbot_core)
1) SBC
Jetson Nano 2G
ros: melodic
arduino: ESP32
2) リモートPC
Ubuntu Mate 18.04
ros: melodic
2. 処理の流れ。
1) SBC での処理。
i) depthai_examples の stereo_node.launch の、stereo_publisher node の部分を動かして、
/stereo_publisher/stereo/depth を publish させます。
ii) rtabmap_ros で、上記、 /stereo_publisher/stereo/depth を取り込んで、3D-Map をpublish させる。
2) リモートPC の Rviz で動作を確認します。
3. rtabmap、 rtabmap_ros への depthai の組み込み。
方法は、自作 Turtlebot3 自立走行に向けたプログラム。#5 に記載しています。
但し、rtabmap へ、depthai を組み込まないといけないので、
rtabmap/CMakeLists.txt を修正します。
line 199 option(WITH_DEPTHAI "Include depthai-core support" ON)
4. 取り敢えず、launch ファイルです。
ベースは、rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot
6.3 Turtlebot3 on Melodic and Noetic
demo_turtlebot3_navigation.launch です。
~/catkin_ws/src/rtabmap_ros_my/launch/rtabmap-nishi_depthai_test1.launch
公開しています。free-soft/rtabmap_ros_my/launch/
注1) 今回は、実機での使用なので、
<param name="use_sim_time" type="bool" value="False">
を必ず付けます。そうしないと、クロックが出てきません。理由は不明です。
6. 問題点。
1) rtabmap_ros は、3D-Map の表示のために、rgb データが必要みたいです。
取り敢えず、今回は、
rgb/image = /stereo_publisher/right/image
rgb/camera_info = /stereo_publisher/right/camera_info
を使ったので、白黒です。将来は、真ん中のカメラの rgb を使えば、カラーになりそうです。
2) move_base の障害物判定の指定。
~/catkin_ws/src/turtlebot3/turtlebot3_navigation/param/costmap_common_params_waffle.yaml
で、いまは、scan の方を使っています。
scan の方を指定すると、実際 scan(Ladir) がなくても、無視してくれるみたいです。
observation_sources: scan
#observation_sources: point_cloud_sensor
が、observation_sources: point_cloud_sensor を有効にして、実際のTopic を渡さないといけません。
Depth to PointClouds2 の変換が必要です。これには、rtabmap_ros/point_cloud_xyz が使えそうです。
試しに、 stereo_node.launch へ組み込んで、変換ができるか確認した後で、組み入れたいです。
動作確認が出来たので、組み込みました。
それに伴って、costmap_common_params_waffle.yaml を修正します。
~/catkin_ws/src/turtlebot3/turtlebot3_navigation/param/costmap_common_params_waffle.yaml
以上で、なんとか、自作Trutlebot3 で、oak-d-lite(Depth データ) を使って、rtabmap_ros が動かせるようになりました。
今までは、自作 Stero Camera での、rtabmap_ros だったので、これからは、depth camera を使った、 rtabmap_ros の調整をしないといけないぞね!!
7. OAK-D-Lite(depthai_ros) を使って気づいた事。
1) やはり、/stereo_publisher/stereo/depth の frame rate が余りに遅いこと。
これは、設定で改善するのだろうか?
Jetson Nano 2G のパフォーマンスの低さに原因があるのだろうか?
Camera のキャプションサイズ自体を、もっと小さく出来ないのだろうか?
2) その後のテストで、
Jetson Nano 2G で、 OAK-D-Lite を、直接、USB3.0 ポートへ挿しました。Wi-Fi dongle
は、USB 2.0 にします。
そうすれば、見違える程速くなりました。
stereo_nodelet.launch で、15[Hz] ほどに改善しました。
ただ、自作 Stero Camera の時と同じスピードです。(但し、 320x240 ?)
Oak-D-Lite(dephtai_ros) は、640x480 で、depth データです。
これなら、Stereo Camera と同じように、foxbot_core3 で、/stereo_publisher/stereo/camera_info を Subscribe して、
同期を取って TF データを出力できそうです。
3) 但し、rtabmap_ros で使うと、 /stereo_publisher/stereo/depth は、1[Hz] 程度に下がります。
/stereo_publisher/right/image は、30[Hz] なのに、/stereo_publisher/stereo/depth は、rtabmap_ros が稼働すると、1[Hz] に下がるのは、
depth データの変換を、OAK-D-Lite は、カメラ側のハードで行っていないのか?
だとしたら、がっかりぞね!!
Intel RealSense D435 等は、カメラ側のハードで行っているのではないのか?
真ん中に、赤外線距離センサーがついているので、ハードで対応しているのでは?
OAK-D-Lite 購入は、失敗だったか?
現状では、Jetson nano 2G には、rtabmap_ros with OAK-D-Lite は、荷が重い?
自作 Stereo Camera と大差ないぞね!! 残念。
OAK-D-Lite は、キャリブレーションデータが内蔵されているので、自分でキャリブレーションする必要が無い点だけの違いか?
訂正。
Jetson Nano 2G で、/stereo_publisher/stereo/depth は、やはり、15[Hz] で取れているみたいです。
rostopic hz コマンドを使う時は、注意が必要みたいぞね。
同コマンドで、トピックを調べる時、同じトピックをサブスクライブしているNode が他にあると、後からオープンしたほうが、
ロックアウトに近い状態になるみたいです。
rtabmap_ros を動かすと、その現象が出てくるみたいです。
これを解決しないと、さくさく動かないようぞね!!
どうやら、rtabmap_ros/rtabmapviz が、同時に /stereo_publisher/stereo/depth をSubscribe しているようです。
これを起動しなければ、OK のようです。
そうすると、10[Hz] ぐらいで、取り込めているようです。
注) この時も、rtabmap_ros/rtabmap node が、同時に Subscribe しているので、もしかしたら、15[Hz] かもしれません。
Jetson Nano 2G の CPU 負荷は、30[%] 程のようです。
だとしても、OAK-D-Lite の Depth 処理は、CPU側で ぐりぐり行っているように思います。
と言うのも、PC だと 30[Hz] でているので、CPU 能力の違いが、Rate の差になっているので。
前述の、OAK-D-Lite と、Wi-Fi Dongle を、USB3 Hub Port に同時に挿しても問題ない様です。
やはり、OAK-D-Lite の購入は、少し後悔がします。
4) 映像のタイムラグが大きすぎ。
Rviz で、モノクロ映像(/stereo_publisher/left/image 、/stereo_publisher/right/image)を見ていても、カメラで写した対象物が、
Rviz 上に出てくるまでに、タイムラグが有り過ぎます。
自作 Stereo Camera だと、タイムラグなしで、Rviz 上に表示されたのに。
OAK-D-Lite では、なんと遅いことか。
/stereo_publisher/stereo/depth は、さらに遅くなります。
これでは、rtabmap_ros で使うには、不向きです。
実際に使ってみて、あまり良い結果はでていません。
ただ、コンパイルする時に、-g オプションが付いたままなので、これを外すと良くなるのか?
結論を言えば、プログラムの作りが悪いような気がする。
uvc_camera だと、実測、15[Hz] だけど、タイムラグはそれほど感じない。
カメラの前に手をかざせば、すぐ映像に出てくる。
だが、OAK-D-Lite は、それが無い。
OAK-D-Lite は、モノクロではあるが、30[Hz] でキャプション出来ているので、ハードの処理自体は、遅くは無い。
但し、データが伝わってアプリケーション側に来るまでに時間がかかっている様だ。
uvc_stereo_camera だと、ドライバーライブラリーに、キャプションデータを直接取りに行っているようだが?
https://github.com/ros-drivers/camera_umd/blob/master/uvc_camera/src/stereo.cpp
stereo_nodelet.cpp は、depthai-core のライブラリーとのデータ通信に、Pipe を使っているようなので、
そこが遅いのか、そこで、タイムラグが生じているのか?
送り側が、どんどこ送信しているのに、受け側が遅いので、パイプのなかでデータが詰まっているのかな?
上記パラメータの、30 が気になる。
これは、キューのサイズなのか? キャプションの Rate なのか?
Rate であれば、15 に下げれば、タイムラグは無くせるのか?
結局、ハードの問題でなくて、depthai-ros-examples のプログラム例が余り良くないのが、今回の問題点か。
プログラムなんて、どうとでも作れる。但し、一番最初にどのように作るかの発想が
大事だ。
全体の構成を見間違えると、いいものは出来ない。
プログラムは、Simple is Best!! に尽きる。
誰か、もっと良い、depthai-ros-examples を公開してくれないものか?
4) その後。
最新の github をダウンロードして試したら、改善されているような気がします。
PC上 ではあるが、カメラの前に手をかざすと、すぐに表示される。ようです。
やっぱり、Jetson Nano 2G では、タイムラグ有り過ぎです。これでは、rtabmap_ros では、使えません。
結局、stereo_nodelet.cpp のパイプのバッファー数=1 にしてみました。
そうしたら、タイムラグが結構改善されました。
しかも、/stereo_publisher/left/image フレーム速度は、29[Hz] 位出ていそうです。
/stereo_publisher/stereo/depth は、 15-14[Hz] 位でています。
注1) 両者の同期が取れていない点が、問題としてありそう。
注2) やはり、オーバヘッド有り過ぎ、CPUリソースの使いすぎとしか言えません。
たかが、Camera のデータを取るのに、Pipe は無いだろう。
Rtabmap_ros の方に、できるだけCPUリソースを使いたいので、Camera の部分でこれ程、
CPUリソースを使われたらどうしようもない。
注3) BridgePublisher.hpp の中に、Pipe を使わない method があるのだろうか?
そもそもの、depathai coe のライブラリーとのインターフェースが、socket 経由みたい。これならだめだ!!
MonoCamera のプログラム Examples があるみたい。
MonoCamera
mono_preview.cpp
stereo_depth_video.cpp
しかし、これをベースにすれば、もっとシンプルなプログラムができそう?
とにかく、軽くて、/right/image と、 /depth が同期取れるプログラムができるのかも?
rgb_depth_aligned.cpp
こちらだと、rgb カメラにアクセスできそう。
少し、depthai-core のexamples を試してみてもよさそう。
rtabmap_ros でも使えそうですが、ほとんど、自作 Stereo Camera 版と同じレベルか、それよりも悪いかも。
やはり、Jetson Nano 2G and rtabmap_ros で使うには、がっかりの性能です。
Intel Realsense D435 が、全然よさそうです。
OAK-D-Lite の使用目的が、Object Detection 等、違えばまた良いのかもしれません。
5) image_transport_plugins が入っているれば、Publish 画像が自動で圧縮されて使えるようです。
/stereo_publisher/left/image/theora
/stereo_publisher/left/image/compressed
Rviz では、各画像の右横の、raw / compressed / compressedDepth / theora / で選べるみたいです。
だとすると、ROS では、画像の Publishは、Publish だけでは、実際のデータ部分の発出は、されていない事になるのか。
今までは、画像を Publish した時点で、ROS サーバーに送られているのかと思っていました。
Subscriber が、Subsclibe した時に初めて、データの転送が始まるのでは?
その時点でネットワークのトラフィックが増加するみたいだ。
と言うのも、Rviz で、圧縮された画像だけを見ていると、表示速度が速くなるので。
Rviz で、RAW の画像を表示すると、PC のネットワークのトラフィックが一気に上がります。
JetsSon Nano 2G では、rtabmap_ros も、この圧縮データを使えば、良いと言う事ぞね。!!
今まで、何をやっていたのか? とほほぞね....