ROS2 自作 Turtlebot3 による 草刈りロボット開発。#5 C++ cmd_vel 走行で障害物をよける方法。
今回は、ROS2 自作 Turtlebot3 や、Gazebo Turtlebot3 で、C++ cmd_vel でコントロールしているときに、障害物を検知する機能を組み込んでみました。
方法としては、Stero Camera、Lidar、Depth Camera を入力として、Local Costmap に
障害物が、Topic として出てくるので、
それを、定期的(1 [sec]) に取りこんで、ロボットの前方に障害物が投影されたら、ロボットの方で、なんらかの対応処理をする。
Navigation2 を使えば、かんたんですが、 rpp 等を使うと、まずロボットがまっすぐ走行しません。
草刈りロボットとしては、障害物が無い間は、常に直線上を走行させたいので、cmd_vel で走行させています。
この時に、障害物が現れたら、Nav2 走行に切り替わって、障害物を避ける動きを組み込んでみました。
cmd_vel 走行のみの場合は、そこで、障害物がなくなるのを待つか、走行処理を終了させる
か、どちらかになります。
プログラムとしては、大体、できているので、後日 github にでも公開します。
github に公開しています。
tosa-no-onchan/turtlebot3_navi_my
具体的には、AutoMower の中に組み込んでいます。
go_auto_mower.cpp -> pro_control_mower.cpp
turtlebbot3_amcl_scan.launch.py で実行できる。
上部のコメント行に従って起動すれば、OK
---> go_auto_mower.launch.py
AutoMower で、障害物がない場合は、cmd_vel モードで走行して、障害物が現れたら、navi モードで走行します。
具体的には、ProControl::move_abs_auto_select() をコールして処理しています。
mulit_goals4_nav2.cpp の方にも組み込んでいます。
こちらは、navi モードと、cmd_vel モードの自動選択になります。
上記、コマンドで、自動で切りかわります。
multi_goals4_cmd_vel.cpp の方にも組み込みました。
こちらは、cmd_vel モードのみの組み込みになります。
set dumper ON を発行したあとに、RobotDriveCmd_Vel::go_abs() を実行すると、local costmap を、逐次チェックするようになります。
注) ただし、今は、このときに、障害物が現れたら、そこで、走行を終了させてしまうので、注意が必要です!!
結論を言えば、RobotDriveCmd_Vel::go_abs() で、一括して処理しています。
ここで、local costmap をチェックする thread を起動して、ロボットの進行方向にobstacle の点 が検出されたら、go_abs() を stop させます。
これだけです。
下記が、AutoMower で実行中の、Rviz2 の画像ぞね。
i) mat_bin_map: local costmap を、cv::Mat で、Binary 画像で取りこんだもの。上下逆になる。
注) resolution:0.01 -> 1 [dot] = 1[cm]
ii) mask: ロボットの前方の障害物をチェックするMask。
iii) result2: i) と ii) の And の結果。障害物が検知できたら、白dot が出てくる。
