ROS2 自作 Turtlebot3 による 草刈りロボット開発。#4 草むらの判定

例えば、
1) 入力を、/cloudXYZ:"PointCloud2" とした場合に、Object Detection の Box 情報も取り込んで、
Box の部分の pointcloud2 のデータは、取り除く処理をすれば、良い。ただ、それだけでOKか?

となれば、plugin: "nav2_costmap_2d::ObstacleLayer" を自分で改造すればOKか?

2) /cloudXYZ:"PointCloud2" を出しているのは、rtabmap_util / point_cloud_xyz なので、こちらに、Box 情報を取り込んで、
Box部分の、PointCloud2 を消す処理を加えるか?

もっと上位レイヤで処理するのであれば、Local CostMap レイヤーと同じレベルで、Local SafeMap と言うものを加えて、
こちらが出す、 Local SafeMap の部分は、通行できると判定させる?

いずれにしても、先ずは、草むらを判定するモデルを作らないといかんぞね。
それには、学習データが必要じゃ。
ロボットの視点から撮影した、草むらの画像をいっぱい集めないといかんぞね。
これが、一番難儀じゃ。

参考にまでに、下記資料も目にした。
AI による画像認識を用いた植物種判別手法の基礎的検討

2. 雑草の学習画像の収集。
学習画像としては、ロボットのカメラ位置からの草むら画像が必要ぞね。
最終的には、カメラ片手に、草むらを撮影しないといかんぞね。

しかしその前に、google で雑草の画像をスクレイピングしてみたぞね。
公開されている、Python Script を参考に、みよう見まねで、今回始めて作ってみました。
scraper3.py

一応、サムネイルより大きい画像がダウンロードできました。
"雑草" で画像をスクレイピングし、800件ほどダウンロードしたけれで、結局使えたのは、130個ほどでした。
やはり、自分でカメラで撮影して集めないといかんみたいぞね。

labelImg で、YOLO形式のアノテーションを作って、Keras V3 Object detection with Vision Transformers で学習させました。
本当に短時間の学習でOKです。が、雑草の部分を一応検出できるようです。
Transformers 恐るべし。
これなら、学習データを作って、学習させて、結果を確認して、
その結果から、再度、学習データを手直しする。この反復学習が簡単にできる。

後日、ロボット視点の雑草の写真を撮影して、それを学習データに追加してやれば、精度は上がるので、これは、ここまで。

しかし、このモデルは、OrangePi5 辺り動かせるのか?これが問題じゃ。

後で、github にアップしちょきます。
tosa-no-onchan/annotation

3. ROS2 の local costmap での利用。
1) 最初は、簡単そうな、 rtabmap_util / point_cloud_xyz への組み込みを試してみる。
改造の要点。
現状、入力は、depth/image か、disparity/image なので、これに、object detection を追加して、草むらであれば、
その Box 部分を、出力の cloud (sensor_msgs/PointCloud2) から消してしまう。
ソースとしては、
rtabmap_util/src/nodelets/point_cloud_xyz.cpp
の、#163 void PointCloudXYZ::callback() 辺りか?
まあ、あんまり良くわからん。

181 cv::Mat depth = imageDepthPtr->image;

上記コードの、cv::Mat depth に、Box 情報を取り込めばよいか?

下記コードを実行した後の、 pclCloud から、box 部分を無効にすればよいのか?

pclCloud = rtabmap::util3d::cloudFromDepth( depth, model, decimation_, maxDepth_, minDepth_, indices.get());

もし、それでよければ、現状、depthMsg 、 cameraInfo のトピックを callback で受信しているので、
それに、 Object detection のトピックも加えれば、なんとかできそうじゃ。

試しにソースを修正して、
Object Detectionのトピックを出すダミープログラムを作って、常に同じ Box を出させて、それを、使って、
この point_cloud_xyz.cpp の出力を、チェックすれば、動作確認ができそうじゃ?

4. 試しに組み込んでみる。
/rtabmap_ros/rtabmap_util/src/nodelets/point_cloud_xyz.cpp をコピーして、 point_cloud_xyz_my.cpp を作ってみました。

組み込み場所は、
void PointCloudXYZ::callback()
void PointCloudXYZ::callbackDisparity()
の、受信メッセージを、cv::Mat depth、disparity に取り込んだ後に、
Detect(vision_msgs::msg::Detection2DArray) メセージを、非同期に取り込んで、それを、depth, disparity に cv::rectangle()で潰してなんとかできました。

動作確認は、Detect(vision_msgs::msg::Detection2DArray) メセージを出す、ダミープログラムを作って、
それに、 oakd-lite の depth メッセージを入力にして、detect されたボックスが、cloud の出力からマスクされるのか確認しました。

なんとか、できました。
後は、 local costmap と連動させて、実際に使ってみないといかんぞね。