2022年5月14日土曜日

BOSCH BNO055をROSで使ってみた。

 LiDARをROSで使えるようにしたのはいいんだけど、SLAMのサンプルを試してみたらやっぱりIMUもあったほうが面白いんじゃないかということでROSで使えるIMUを探してみた。

MPU-6050(6軸)とかMPU-9250(9軸)はArduinoとかでもよく使われていて、DMPを使えばフィルタして安定しているデータを得られるんだけど、ラズパイとかだとI2Cで直結できたりするかもしれないけど、PCだとちょっと不便。WitMotionからカルマンフィルタ通したデータがワイヤレスやシリアル通信で得られるモジュールも出てるっぽいけど。

そこでカルマンフィルタ付きのIMUを探していたら、BoschのBNO055っていうセンサーがマイコン内蔵で出力がI2CとUARTが選べるという。これならUARTでUSB変換通せば簡単にPCに取り込めそう。更にはROSのドライバもあるようだ。

秋月だと1680円だし、WitMotionのモジュールよりも安い。ケース無いけど。

秋月のこういったモジュールは結構DIPサイズに変換してくれるのが多い気がする。
ブレッドボードに挿しやすくて便利。
ジャンパは表面に全部揃ってる。最初からI2Cモードになってる。
ジャンパには0604のジャンパ抵抗が乗ってるぞ…
両面実装なので、ユニバーサル基板に装着する際もベタ付にはできなさそう。

レベルコンバータとレギュレータも内蔵してるので5Vでも3.3Vでも使用できるみたい。入力した電圧にレベル変換されるっぽい。今回はUSBシリアル変換にCP2102を使用したので3.3Vで使用することに。
まずはROSでUARTで使用するためにI2CモードからUARTモードにジャンパを変更。J5のジャンパ抵抗をハンダで外して、J3にハンダを盛ってジャンパする。
あとはUSBシリアル変換アダプタにつなぐだけ。

まずはrivzでIMUを使用できるようなプラグインをapt-getでインストール。
sudo apt-get install ros-$(rosversion -d)-rviz-imu-plugin

ROS用のドライバにはros_imu_bno055を使用させてもらった。
インストールは説明されている通りのコマンドコピペでcatkinベースでビルドできた。

rivzでそのままセンサーの状態が表示できた。
さすが内蔵のマイコンでフィルターを掛けているだけあって安定している。ArduinoでMPU-6050の生データを見ていたときはZ回転がかなりドリフトしていたのでIMUは難しそうと感じていたけど(DMP使ったら問題なかったけど)、簡単にフィルタ後のデータをUARTで出せるBNO055は流石だ…

最初はArduino+MPU-6050でDMPつかってUARTで出力とか考えていたけど、BNO055つかってみて正解だったかも。

2022年5月10日火曜日

ロボット掃除機のLiDARでHector SLAMを試してみた。

 前回ROSで使えることがわかったロボット掃除機のLiDAR LDS-006でHector SLAMが使えるかどうか試してみた。IMUとかは無しでロボット掃除機に搭載されているレベルのLiDARだけでどのぐらい自己位置推定ができるのかどうか。

Hector SLAMの使い方はRPLidarでHector SLAMを使っている人のものを参考にさせていただいた。検索すると結構出てくるのでそちらは割愛。
前回のROSのテストをした際にframe IDをlaserにしておいて正解だったかも。ほぼコピペでそのまま動いた。

とりあえず自分自身がじゃまになるんじゃないかということで頭の上にLiDARをのっけて少し動いてみた。
緑色の線が動いた軌跡。こんな簡単にSLAMが試せるのか…

ただしLiDARを回したり上下したりするとこんな感じに。スキャンレートが遅いからなのか、回転には弱い気がする。設定が悪いのか、LDS-006の限界なのか…
IMUとかのセンサを組み合わせたサンプルとかあるのかな?回転方向とか傾きとかはIMUのほうが検出しやすそうだし。


2022年5月8日日曜日

LDS-006 LiDARをROSで使ってみた。

 この前購入した格安LiDAR、とりあえず動くところまでわかったので今度はROSで使えないかどうか調べてみた。

ROSで使えるLiDARで似たような形状しているやつだとRPLidarとか、同じようにUARTで通信しているタイプでLaserScanメッセージ対応のROSドライバベースで作れないかと、LiDARの仕様書や使い方のページとにらめっこして、近いプロトコルないかなーとか探してみた。

その過程で見つけたNeato XV-11っていう掃除ロボットに乗ってるLiDARがLDS-006のプロトコルにそっくりということを発見。

そっくりということは実は同じプロトコルでLDS-006の解析結果が間違っているんじゃないかという疑いが…
たしかに他のLiDAR用に作られたツールを改造してそこそこ使えそうな気はしていたんだけど、定期的に変なデータが来て値が飛んでいた。

そこで、解析してみるとLDS-006の距離データの部分に実はエラーフラグとエラーコードが含まれていたっぽい。これはXV-11の方のプロトコル解析結果を見てもしかしてと思って、距離のデータのHigh Byteの上位2ビット目を観察するようにしてみたら判明した。上位2ビットが0以外になると実は距離データのLow Byteがエラーコードになるようで、それが距離データとしては特定の角度で一定の変な値が出ることを確認。

というわけでLDS-006はXV-11のプロトコル互換ということで、XV-11がROS対応しているのをいいことにそのままXV-11用のドライバでROSで動かしてみた。
LDS-006にはモータ制御のコマンドが存在するのでモータ制御のコマンドは自分で実装するか手動で送信するしか無いけど。("startlds$"で回転、"stoplds$"で停止)

2.0mmピッチのコネクタも一応2.54mmピッチに変換して(4pinぐらいなら半田でなんとかなる)USB-Serial変換アダプタはCP2102搭載のものを使用した。

使用したドライバはxv_11_laser_driver
これはパッケージも提供されているようなのでapt-getでインストールできた。

あとはroscoreを動かしたあとに別なターミナルで

echo "startlds$" > /dev/ttyUSB0

をするとLDS-006が回り始める。
次に

rosrun xv_11_laser_driver neato_laser_publisher _port:=/dev/ttyUSB0 _firmware_version:=2 _frame_id:=laser

これでROSでLiDARのデータを解析してROSに渡してくれる。firmware versionはXV11のLiDARは通信プロトコルが2種類あるので2のほうがLDS-006と互換のほう。frame idはデフォルトでneato_laserになってるんだけど、laserにしたほうがRPLidarとかの使い方のサイトを参考にしたときにそのままコピペできるので便利だと思ってこのオプションを追加して変更した。

/dev/ttyUSB0は環境に合わせて変更。Permission Errorが出たので

sudo chmod a+rw /dev/ttyUSB0

してやった。適当だけど動いたので良し…(あとでちゃんとグループ調べて追加しないと…

あとはRVizを開いてFixed Frameにlaserって入れれば見れるはず。

LDS-006 ROS
データの取得ができた。やっぱりエラーフラグもちゃんと処理してるのか、動かしても変なデータが出ないのでいい感じ。
xv11が5mなので5mまでしか見ていないかもしれないけど、LDS-006がどのぐらい届くかは見てみないとなぁ
ちなみに分解能はやっぱり1°で、スキャン周波数は5Hzぐらいだった。
ROS対応でこのぐらいの性能なら1万円ぐらいの低価格帯のLiDARレベルかもしれない。

とりあえずRPLIDARと同等ぐらいにはROSで扱えそうな気がしてきたので、RPLIDARの情報を参考にSLAMとか試してみようかな。