rtabmap_rosの最近のブログ記事

自作 Turtlebot3 自律走行に向けたプログラム。#10
--- local_costmap をプログラムから直接アクセスして、障害物の判定に使う ---

自作 Turtlebot3 自律走行に向けたプログラム。#9 で、move_base を使わないで、
cmd_vel を操作して、ロボットを移動させる事ができたの、今回は、Turtlebt3 の local_costmap をc++ プログラムから、
直接アクセスして、障害物の判定に使う方法を考えてみます。

と言うのも、Single USB Stereo Camera から stereo_image_proc -> rtabmap_ros/point_cloud_xyz を通すと、
障害物の PointCloud2 が得られるので、
それを、 local_costmap の入力に与えると、 Stereo Camera による障害物の判定結果が、自動的に、local_costmap の
出力に出てきます。

1. 開発環境。
Turtlebot3
Gazebo Sumilation または、自作 Turtlebot3 (foxbot_core3.ino)

PC
 Ubuntu Mate 20.04
 ROS noetic
 rtabmap_ros
注) 今回から、ROS noetic での開発になります。

SBC
 Jetson Nano
 Single USB Stereo Camera
 JetPack4.6 and Ubuntu 20.04
 ROS noetic
 rtabmap_ros

または、
Gazebo & Turtlebot3 burger with Stero Camera

自作 Turtlebot3 自律走行に向けたプログラム。#8
--- Turtlebot3 SBC(Jetson Nano 2G) で、rtabmap_ros で、色々試す。#2 ---
補記) move_baseのローカルプランナーを変更する 。

自作 Turtlebot3 自律走行に向けたプログラム。#7 の続きです。

Spark Fun DMP 6軸フュージョンをキャリブレーション でロボットの前傾姿勢 (IMUの観測値) の補正が出来なくて、
IMUの取り付けスペーサーの両面テープで、前傾を調整して、それから、ロボットを平らな所に置いて、
$ rostopic echo /odom_fox | more
で、見てみると orientation:y = 0.013
で少し前傾しているので、最後の手段、foxbot_core3.ino で、むりやり、q[1] -= 0.013
をして、やっと、前傾がなくなりました。

注) ここは、クォータニオンの回転の計算をしないといけないようです。
クォータニオン(四元数)を使用して座標を回転させる
P2 = Q x P x 共役Q
クォータニオン積を2回行う様です。
今は、手抜きの計算なので、ロボットが、後ろ、179[Deg] から -179[Deg] へ回転する時、変にぶれます。by nishi 2022.6.12

後、ロボットの起点を平らな所に移して、これで、rtabmap_ros で試すと、Rviz の画面で、以前ほどは、沈み込はなくなりました。
ただ、高さの計測が出来ているかと言えば、まだまだその精度には、なっていません。

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 で動作を確認します。

Jetson Nano 2G ROS Oak-D-Lite

やっと、OAK-D-Lite を購入できたので、
Jetson Nano 2G の ROS(Melodic) で、Oak-D-Lite(depthai-ros) を使えるようにしてみました。
参考は、 luxonis / depthai-ros です。

環境:
1) 実行 SBC
Jetson Nano 2G: jetpack4 (ubuntu 18.04 相当)
OpenCV4.1.1
注) OpenCV4.x が必要とのことです。
Jetson Nano 2G OpenCV4.1.1 Build を参考にして下さい。
Ubuntu 18.04 は、OpenCV3.2 です。
後、他のROSパッケージも、OpenCV4.x 対応にしないと行けないので、下記を参考にして下さい。
自作 Turtlebot3 自立走行に向けたプログラム。#5
2) Rviz の確認用 リモートPC
Ubuntu Mate 18.04
ROS:melodic
注) こちらにも、同じように、depthai-core、 depthai-ros をインストールします。
但し、OpenCV3.2 なので、完全にはインストール出来ませんが、Rviz での確認に必要になります。

自作 Turtlebot3 自律走行に向けたプログラム。#7
--- Turtlebot3 SBC(Jetson Nano 2G) で、rtabmap_ros で、色々試す。 ---

自作 Turtlebot3 自律走行に向けたプログラム。#5 の続きです。

14. たまに、IMU データの音信不通がありますが、それでも大夫安定して動く様になったので、 rtabmap_ros の使い込みをしています。
その中で、判った事を書いています。
注) IMU データの音信不通 の原因がわかりました。/odom_fox の出力を、camera_info に同期して出す
様に組み込んだ処理にバグがあるようです。
いまは、この機能を外して使用します。by nishi 2022.5.18

1)
RGBD/OptimizeMaxError=3.0(default) - 3.3
この値によって、tf-odom の補正の量が左右されるみたいです。
デフォルトの 3.0 より大きめの 3.3 にしています。動かしていて、ワーニングが出たので少しだけ大きくしました。
試しに、2.1 など、小さくして試しましたが、そうすると、tf-odom の補正がされなくなりす。
Rviz で見ていると、tf-odom が補正されると、黄色の線が表示されるようです。
tf-odom が補正されないと、2M 行って帰ってきた時、出発点から、随分ずれて止まります。

自作 Turtlebot3 自律走行に向けたプログラム。#5
--- Turtlebot3 SBC(Jetson Nano 2G) で、rtabmap_ros で、Kp/DetectorStrategy=6(GFTT/BRIEF) を使う方法。 ---

副題: OpenCVにcontrib を組み込んで、rtabmap の再ビルド。

自作 Turtlebot3 自律走行に向けたプログラム。#4 の続きです。

rtabmap_ros で、Kp/DetectorStrategy=6(GFTT/BRIEF) を使うには、OpenCV に contrib を組み込まないといけない。とのことなので、
組み込んでみました。結構大変ぞね!!!
参考は、下記ページ。
introlab/rtabmap_ros

開発環境
Jetson Nano 2G JetPack 4 (Ubuntu 18.04)
Ros: Melodic

自作 Turtlebot3 自律走行に向けたプログラム。 #2
--- 自作 Turtlebot3 Rtabmap_ros で、Odometry と IMU のフュージョンを行う。 ---

前回、自作 Turtlebot3 自律走行に向けたプログラム で、Odometry と IMU のフュージョンが必要だと痛感したので、
今回は、それを試してみます。

Odometry と IMU のフュージョンに関しては、下記を参考にしています。
ROS講座61 位置情報の統合

1. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170

2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2
IMU:ICM-20948 / Switch Science SparkFun のこちら

自作 Turtlebot3 自律走行に向けたプログラム。
--- Python Script で 自作 Turtlebot3 を動かしてみる。 ---

Turtlebot3 の自作 #12 で、自作ステレオカメラ & rtabmap_ros & Navigation が出来るようになったので、
それを使って、自律走行に向けたプログラムの試作をしてみます。

常套手段として、Python プログラム で自作 Turtlebot3 を動かしてみます。

1. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170

2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2

Turtlebot3 の自作 #12
- ROS rtabmap_ros 自作 Stereo Camera で MappingAndNavigationOnTurtlebot を試す 2 --

Turtlebot3 の自作 #11 で作成したプログラムを、今回は、少し改造してみます。
前回は、stereo_node.launch を除いて、全て リモート PC上 で rtabmap_ros を動かしました。
今回は、確認の意味を込めて、SBC 上で、rtabmap_ros を動かしてみたいとお思います。
1. SBC で動かす、node、launch。
turtlebot3_bringup/turtlebot3_robot.launch
stereo_node.launch
stereo_image_proc
rtabmap_ros/rtabmap

2. remote PC で動かす、node、launch。
roscore
turtlebot3_bringup/launch/turtlebot3_remote.launch
turtlebot3_navigation/launch/move_base.launch

自作 Turtlebot3 foxbot_core3

3. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170

2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2

Turtlebot3 の自作 #11
-- ROS rtabmap_ros 自作 Stereo Camera で MappingAndNavigationOnTurtlebot を試す --

今回は、自作 Turtlebot3 に搭載した、自作 Stereo Camera を使って、
rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot を試してみます。
上記ページの 6.1 Turtlebot3 on Melodic and Noetic を試してみます。

これが、上手く動けば、rtabmap_ros で、自作 Stereo Camera を使って、Mapping しながら、ナビゲーションができるようになります。

自作 Turtlebot3 foxbot_core3

1. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170

2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2

Turtlebot3 の自作 #10
-- ROS rtabmap_ros 自作 Stereo Camera で StereoOutdoorMapping を試す --

今回は、自作 Turtlebot3 に搭載した、自作 Stereo Camera を使って、 rtabmap_ros/Tutorials/StereoOutdoorMapping を試してみます。

自作 Turtlebot3 foxbot_core3

1. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170

2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2

Turtlebot3 の自作 #9
-- ROS rtabmap_ros 自作 Stereo Camera の搭載 --

ROS rtabmap_ros 自作 Stereo Camera #2 で確認した機能 (rtabmap_ros/Tutorilas/SetupOnYourRobot 2.9 Stereo B) を、
Turtlebot3 自作機(Foxbot_core3) へ、搭載してみます。

自作 Turtlebot3 foxbot_core3

概要:
1) SBC(Jetson nano 2G) では、stereo_node.launch、 turtlebot3_robot.launch を動かす。
2) リモートPC で、stereo_image_proc、stereo_odometry、 rtabmap_ros を動かす。
3) リモートPCで、teleop_key を使って、自作 Turtlebot3 を動かして、Wi-Fi経由で、Stereo 画像をリモートPCで受けて、
Rtabmap_ros で、2D Mapping を作成する。

1. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170

2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2

2. SBC(Jestson nano 2G) での準備。
1) 自作 Stereo カメラを、車体に固定します。
2) USBカメラ x 2 を、Jetson nano 2G に接続して、
$ roslaunch uvc_camera stereo_node.launch を実行して、
カメラの左右が正しいことを確認します。
3) Stereo キャリブレーションを行います。

ROS rtabmap_ros 自作 Stereo Camera #2.

ROS rtabmap_ros 自作 Stereo Camera に続いて、
今回は、rtabmap_ros/Tutorilas/SetupOnYourRobot 2.9 Stereo Bを、
自作 Stereo カメラ向けに、アレンジして、試してみます。
と言うのも、 rtabmap_ros/Tutorials/StereoOutdoorMapping で使われている構成のようなので。

開発環境
PC: Ubuntu Mate 18.04
ROS : Melodic
USB Camera: Logi Cool C270 x 2

1. 自作 USB ステレオカメラ のセットは、ROS rtabmap_ros 自作 Stereo Camera をご覧下さい。

ROS rtabmap_ros 自作 Stereo Camera.

rtabmap_ros Tutorial で、Stereo カメラが使える とあるので、自作の Stereo Camera でも大丈夫なのか試してみました。

手元に、Logi Cool の USB カメラ C270 が１個あったので、急遽、もう一本買ってきてためしてみます。
始める前に、rtabmap_ros のパラメータで、
Kp/DetectorStrategy=6 (GFTT/BRIEF)
Vis/FeatureType=6 (GFTT/BRIEF)
がデフォルト設定になっています。これは、OpenCV に contrib を組み込まないと使えない事のようです。
introlab/rtabmap_ros
Kp/DetectorStrategy=8 (GFTT/ORB)
Vis/FeatureType=8 (GFTT/ORB)
だと、contrib は、必要ないです。
後述のドリフトの件は、デフォルトの 6(GFTT/BRIEF) を使っていた所為かもしれません。by nishi 2021.12.11

注1) 後日、改めて、新品を追加購入して、新品の C270n X 2 にしました。
古い C270 と新しい、C270n とでは、色の具合ちょっと異なりました。新しい C270n で揃えると、左右の色が同じになりました。
この違いが、後の、StereoCamera のキャリブレーションと、stereo_image_proc の結果に大きく影響してきます。
自作 StereoCamera を試す時は、必ず同じロットの C270n を2個揃えて下さい。
Rviz の strereo_camera/point2/PointCloud2 の表示で驚く程違いがでます。by nishi 2021.10.24

注2) 左右USBカメラの間隔は、空けないで、隣合わせで取り付けて下さい。by nishi 2021.10.26
間隔 無し=2[mm] で、レンズ間隔=7.3[cm] となって、Rviz の Stereo_camera/PointCloud2 の映像で最短焦点距離は、
57[cm] になりました。
以前は、レンズ間隔が、9.2[cm] でした、その時の最短焦点距離 70[cm] でした。
最短焦点距離 70[cm] で、rtabmap_ros でロボットカーを部屋の中で動かすと、近くの物体が認識出来なくて、
動作が不完全でした。
最短焦点距離を、57[cm] にして、なんとか、部屋の中で、rtabmap_ros でロボットカーを動かせるようになりました。
しかし欲を言えば、もっと短い方が良いです。できれば、最短焦点距離 35[cm] 程が良い様です。

注3) カメラの取り付けは、とにかく正確に!!
cameracalibrator.py で、左右のカメラ映像を見比べて、上下のズレ、傾きを限りなく Zero にしてください。

注4) 上記設定とStereoCamera キャリブレーションがうまく行けば、Rviz で StereoCamera/PointCloud2 を表示して、
カメラの後方からみてみると、結構きれいに表示されます。

注5) 結論を言えば、C270 は、カメラ間隔を狭められなくて、画角も狭い(55度) なので、stereo_image_proc を通した、最短焦点距離が、57[cm] 迄しか短くできない。
なので、部屋の中で、自作 Stereo_camera & rtabmap_ros を試すには、適切ではありません。
部屋の中で試すなら、最短焦点距離を短くできる、もっと画角が広くて、カメラの間隔を狭められるカメラをお勧めします。
Jetson Nano 4G、Raspberry pi 4 であれば、CSI カメラを2個繋げられるので、そちらの方が、簡単に、最短焦点距離の短い Stereo Camera を作れると思います。
画角は、80度以上が必要です。

注6) やはり、ロボットに乗せて、rtabmap_ros で使うのであれば、市販のStereo Camera、Depth Camera
を購入したほうが賢明です。

開発環境
PC: Ubuntu Mate 18.04
ROS : Melodic
USB Camera: Logi Cool C270 x 2

1. USBカメラの動作確認。
1個の場合。
PC に USB カメラを挿して、
$ lsusb
...
Bus 003 Device 009: ID 046d:0825 Logitech, Inc. Webcam C270
...
を、確認します。表示されなければ、
$ sudo update-usbids
行います。
OK であれば、