ROS2 自作 Turtlebot3(foxbot_core3_r2) Rtabmap_ros with Stereo rgbd and GPS. #2

1) 早速、foxbot_core3_r2.ino へ組み込んでみます。by nishi 2023.3.2
foxbot_core3_r2.ino に組み込む時に、subscriber を 2個 (/cmd_vel と /fox_beat)に、したら、実行時にエラーが出る。
micro-ros-arduino の複数 subscriber 使用時のトラブル発生。

foxbot_core3_r2.ino の抜粋。
//---------------------- // create executor //---------------------- // for publisher RCCHECK_PROC("create_entities() #11 : init executor", rclc_executor_init(&executor, &support.context, 1, &allocator)); // for cmd_vel subscriber RCCHECK_PROC("create_entities() #12 : executor_add_subscription executor", rclc_executor_add_subscription(&executor, &cmd_vel_subscriber, &cmd_vel_msg0, &commandVelocityCallback, ON_NEW_DATA)); //if(use_beat==true){ // for fox_beat subscriber add by nishi 2023.3.2 by nishi RCCHECK_PROC("create_entities() #13 : executor_add_subscription executor", rclc_executor_add_subscription(&executor, &fox_beat_subscriber, &fox_beat_msg0, &fox_beatCallback, ON_NEW_DATA)); //}
2番めの rclc_executor_add_subscription() で、rc=1 エラーになる。
google で検索したら、
Multiple subscribers does not work. #513 に出ていた。
rclc_executor_init() の handle 数を、 1 -> 2 に増やせば解決した。
rclc_executor_init(&executor, &support.context, 2, &allocator);

2) 再度、走行テスト。by nishi 2023.3.3
あんまり芳しくない。
結局、foxbot_core3_r2.ino の /fox_odom、 tf、 imu のrate を、 15 -> 12 に下げた。
ekf.yaml
frequency: 4.0
に下げた。
これで、大部良くなった。やれやれじゃ。

3) 走行テスト。 by nishi 2023.3.5
結局、ESP32 - micro-ROS agent (Jetson Nano 2) 間の通信が切れるのが諸悪の根源みたいだ。
特に、今回は、robot_localization/ekf_node で、入力に、imu を使っていると、問題が顕著になる。
ESP32 からの、/imu トピックが切れると、どうも、ekf_node が、古い値をそのまま計算に使って、次のロボットの向きを予測して、
tf に出力しようとするので、ここで問題が大きくなる。
この時、ロボットが、同じ様に動いていればよいのだが、止まっていた場合、誤差が大きくなる。
ekf が、将来の値を予測するのは、本来の目的なので責められない。なので、おんちゃんは、ekf の入力から、/imu を外すことにしました。
foxbot_core3_r2 の、/odom_fox の中に、ICM-20949 DMP から得たロボットの向きが、ちゃんと入っているので、問題はありません。

将来、ESP32 - micro-ROS agent 間の通信が安定したら、もとに戻します。

ESP32 には、USB device が入っているので、USB CDC として、Jetson Nano 2G の USBポートに、直で繋げられれば改善したりしないだろうか?
USB CDC

このためには、micro_ros_arduino の firmware を、自分で改造しないといけない。これが また、よくわからん。
source で提供しておいてくれればよいのだが、ライブラリでしか提供していない。
そのためには、docker でソースをダウンロードして、手直しないといけないのか。
micro-ROS/micro_ros_arduino

micro_ros_arduino/src/default_transport.cpp の中で、Serial を、USBSerial に変えるだけでよいのだろうか?

4) 走行テスト。 by nishi 2023.3.7
たまに、突然、ロボットが目的地点を通り過ぎて、暴走する。
原因を、探っていくと、どうやら、/tf の base_footprint 辺りが、更新されなくなるみたい。
これを出しているのは、今は、robot_localization/ekf_node だ。
多分、robot_localization/navsat_transform_node 辺りのトピックが遅くなるのが原因だと思う。

SBC - Remote PC 間の通信は大丈夫みたいだ。
/tf が更新されていなのに、navigation2 が其れを認識せずに、ただただ、ロボットを目的地に動かそうとするのが、原因だ。
yaml ファイルの中に、tf が、一定期間、更新されなくなったら、ロボットを停止する指定があるか、見てみたが、見当たらない。

仕方がないので、C++ プログラム multi_goals4_nav2.cpp の中で、/tf map->base_footprint が更新されるの常にチェックして、一定時間更新されなくなったら、navigation2 に cancel を通知するように改造します。

これで、暴走はなくなったが、/tf lost は、結構頻繁に生じているので、いまのままでは、目的地にたどり着けない。
どうするか?

/tf が、1[秒] 程、止まったら、Heart Beat を止めて、 ロボットを止めて、 /tf が回復したら、 Heart Beat を再開して、ロボットを動かせる。
これで、なんとか、暴走はなくなり、目的地も目指せます。
後は、/tf が更新されなくなる問題を、解決すれば、GPS での運転もなんとかできそうです。

どうやら、rtabmap が止まると、tf が止まってしまうみたい。rtabmap が再開すると、 tf が出てきて、ロボットが動き出す。
しかし今までのように、自分の位置を見失うことなく、完走できるようになった。
ただ、時々、tf が止まってしまうので、その間は、ロボットも立ち往生したままぞね。
rtabmap が止まる、原因は、 Single Stereo Camera が止まるのが原因か?
rtabmap が止まると、 tf map->odom 辺りが止まってしまうので、それが理由か?

ともあれ、これで、Rtabmap_ros with Stereo rgbd and GPS で、安心して動かせるようになった。
ROS の時も、同じ症状が出て、使えなかったが、やっと出来た。此処まで来るのに、永かった。

5) 走行テスト。 by nishi 2023.3.12
走行させていると、やはり、Single Stereo Camera のデータが止まってしまう様だ。
プログラム内に、トレース表示を入れて、調べてみると、Camera driver からのデータの取り込みで無くて、
Camera image の Publish 部分で、止まってしまうみたいだ。
でも、更にしらべて行くと、Wi-Fi Lan が切れるのが原因のようだ。
結局 Wi-Fi Dongle を、TP-Link 150[Mbps] に戻したら安定した。

これで、嘘のように、安定して動き出した。
Remote PC 上で、 navigation2 と コントロールプログラム(multi_goals4_nav2) を動かしているので、
Remote PC - SBC 間の WiFi Lan の性能の影響を大きく受けるみたいぞね。

3. おんちゃんのメモ。
Orange Pi 5 と言うのがあるみたいぞね。
こちらの方が、Jetson Nano 2G より、ずっとパワフルみたいじゃ。
これだと、Jetson Nano 2G よりずっと安心か?
OrangePi5を試してみた

ケース& Heat Think & fan
Heat Think & fan

これだと、 SBC 上で、navigation2と コントロールプログラム(multi_goals4_nav2) も動かせそうなので、安定して動かせそう。
そうすると、残る問題点は、ESP32 - Micro ROS Agent 間の通信の品質改善だけになりそうぞね。
これは、 最新の ESP32 に USB デバイスが組み込みになったので、ESP32 の USB 端子と直で、SBC の USB とを繋げれば改善できそう。