open-rdc
ros2のIMUキャリブレーション
ros2でキャリブレーションできるようにします
https://github.com/technoroad/ADI_IMU_TR_Driver_ROS2 box2で使用しているキャリブレーションのros2バージョンを見つけたのでこれでできるようになるかもしれません.
と思ったのですがよくみたら対応しているIMUのバージョンが一個上の物からでした（box2のIMUは対応している） 実際に使用してみて判断しますが, 新しいの買ってもらうかbox2のIMUと交換することが必要かもしれません.
ここらへん過去にも検討されていますので，issueの検索と先輩に質問をしたほうが良いと思います． IMUは一見動いているようで本来の性能を出すためには細かな手順が定められているので，情報をしっかり集めないと時間を無駄に費やすことになります． 記憶では昨年度の段階で解決していた気もするので，状況をしっかり確認しないと２度手間になるかもしれません．
https://github.com/open-rdc/orne-box/issues/74
ちなみに，以下のROSノードを従来使用してきましたが，このソフトウェアをベースとしたノードを使用したほうが良いです． 多数フォークされていますので，ROS2ノードもあるのでは無いかと思います． （これを行わないとかなりハマりますのでご注意ください．詳しい話を聞きたい場合は直接聞いて下さい．）
https://github.com/tork-a/adi_driver
現時点でorne-box3のros2では, IMUは以下のソフトウェアを使用している. https://github.com/masakifujiwara1/adi_driver2
box3のros1ではimuのフィルター後トピックをimu-filter-madgwickで出していて, ros2にも同じものがあることがわかったので試す.
sudo apt install ros-humble-imu-filter-madgwick
実行コマンド
ros2 run imu_filter_madgwick imu_filter_madgwick_node
これを使用するには/imu/data_rawと/imu/magが必要で, 現状は/imu/data_rawしかパブリッシュされていないため, 追加する必要がある.
https://wiki.ros.org/imu_filter_madgwick このドキュメントによると, use_magというパラメータで磁気センサを要求しないようにできるらしいので変更する
ros2 run imu_filter_madgwick imu_filter_madgwick_node --ros-args -p use_mag:=false
実行時のオプションで磁気センサをオフにすることができた.
/imu/data_rawをフィルターにかけて/imu/dataにすることができた.
adi_driver2にはbias補正の機能がついていないようだったので, adi_driver2にbias補正を掛ける用の関数を追加した. https://github.com/kyo0221/adi_driver2/blob/feat/bias_estimate/src/adis16465_node.cpp#L92-L108
bias補正はサービスを投げることで開始されるようになっていて, リストを表示したときに存在していることが確認できる /imu/adis16465_node/describe_parameters /imu/adis16465_node/get_parameter_types /imu/adis16465_node/get_parameters /imu/adis16465_node/list_parameters /imu/adis16465_node/set_parameters /imu/adis16465_node/set_parameters_atomically /imu/bias_estimate
以下のコマンドでサービスを呼び出すことができる
ros2 service call /imu/bias_estimate std_srvs/srv/Trigger
実行結果は以下のようになる requester: making request: std_srvs.srv.Trigger_Request()
response: std_srvs.srv.Trigger_Response(success=True, message='Success')
responseのメッセージがtrueとなっていて, 正常に起動できていることが確認できる.
しかし, imuノードの標準出力に以下のエラーが返ってきてしまう [adis16465-1] [INFO] [1724383769.898636552] [imu.adis16465_node]: bias_estimate [adis16465-1] bias_estimate start [adis16465-1] write_register: Bad file descriptor
bias補正を掛けている関数は以下の関数であり, registerのアドレスが間違っている可能性も考えたが, adis16465のデータシートを見たところ合っているようだった. https://github.com/kyo0221/adi_driver2/blob/feat/bias_estimate/src/adis16465.cpp#L361-L367
@kyo0221 ros2用ではないと思いますが，こちらのissueでIMUのキャリブレーションを行っているようです． https://github.com/open-rdc/orne-box/issues/74
機能が重複することになりますので，既存のものがあるのであれば，そちらを利用したほうが良いと思います．
box2でもimuのキャリブレーションにはadi_driverを使用しているようで, 以下のノードの関数でbias補正のためにレジスタへの書き込みを行っています. https://github.com/kyo0221/adi_driver2/blob/feat/bias_estimate/src/adis16465.cpp#L361-L367
アドレスは0x68で, dataは1でbias補正が開始されるとadis16465のデータシートに記載がありました. こちらはbox2に搭載されているadis16470と同一のアドレスであり, 変更する必要は無さそうです.
書き込みを行っているのはrosを使用していないノードで, ros1のadi_driverとros2のadi_driver2では同一の機能を持っているはずです. rosノードの中でサービスを受け取ることでレジスタに書き込む関数が呼び出されるようになっているのですが, 関数を呼び出した際にメッセージのフィードバックでtrueかfalseを返すようになっています. ros1でもros2でも同じくtrueを返していて, rosノードの書き方にも問題が無いように思えます.
にもかかわらずros2で上記のエラーが発生するのは, box2とはimuの型番が違うからだと考えています.
原因がimuの型番によるものだった場合, box3のros1でもキャリブレーションができていないということになるので, 実験を行いました.
実験内容 bias補正をした場合としない場合のそれぞれで 2号館3階をteleopで1周させた時の/odom/filteredの遷移を見てbias補正が掛かっているか確認する /odom/filteredはodomとimuのデータを統合させたトピック （ros-noeticを使用）
実験手順 box3のbringupのlaunchを立ち上げる 100秒程度待った後, rosserviceで/imu/bias_estimateをcallする /odom/fileteredを可視化して2号館3階を1周する
結果
・bias補正を掛けていないとき
・bias補正を掛けたとき
bias補正は掛かっているように思える結果になった.
@kyo0221 購入記録が残っている型番は，ADIS16470ではなくADIS16475となります． おそらくプロトコルの違いはないと思いますが，念のためお伝えいたします．
IMUの値を確認しました.
orientationを取得してオイラー角に変換するスクリプトを作成して試しました.
・動かしていない状態 https://youtube.com/shorts/3RZ9epqCjbk IMUがドリフトしていて値が常に変動しています. bias補正はしていない状態です.
・90度回転させた状態 https://youtube.com/shorts/L0b_4o4o6F0 値が大げさに変動していて1周したときと同等の出力をしています.
https://github.com/open-rdc/adi_driver/commit/3dff6168e2257ac90d24b2efe324c8e44144cd90
このコミット履歴と同じような対処をして解決しました.
https://github.com/HanLeqi0117/adi_driver/tree/ros2
こちらのリポジトリのadi_driverを使用することでros2を使用してIMUのbias補正ができることを確認しました.
しかし, ロボットを動作させた時にIMUの値が正常に遷移しない問題が発生しました. https://github.com/open-rdc/orne-box/issues/100#issuecomment-2363578510 こちらの内容と同様の問題ですが, その時に変更した箇所を修正しても問題が解決しませんでした.
上記に記載したadi_driverを使用してimuの値が正常に遷移するようにできました. 前回修正した箇所と同様の箇所を変更した時に解決しました. vscodeのdockerプラグインを利用して編集していたのですが, 前回の作業時には変更がうまく反映されていなかったようです.
ros2のシステムでIMUのbias補正ができるようになって正常に使用できることを確認したのでcloseします.