Open kyo0221 opened 1 month ago
yasuohayashibara
commented
1 month ago ここらへん過去にも検討されていますので，issueの検索と先輩に質問をしたほうが良いと思います． IMUは一見動いているようで本来の性能を出すためには細かな手順が定められているので，情報をしっかり集めないと時間を無駄に費やすことになります． 記憶では昨年度の段階で解決していた気もするので，状況をしっかり確認しないと２度手間になるかもしれません．
yasuohayashibara
commented
1 month ago ちなみに，以下のROSノードを従来使用してきましたが，このソフトウェアをベースとしたノードを使用したほうが良いです． 多数フォークされていますので，ROS2ノードもあるのでは無いかと思います． （これを行わないとかなりハマりますのでご注意ください．詳しい話を聞きたい場合は直接聞いて下さい．）
kyo0221
commented
1 month ago 現時点で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にすることができた.
kyo0221
commented
2 weeks ago 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] 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
yasuohayashibara
commented
2 weeks ago @kyo0221 ros2用ではないと思いますが，こちらのissueでIMUのキャリブレーションを行っているようです． https://github.com/open-rdc/orne-box/issues/74
機能が重複することになりますので，既存のものがあるのであれば，そちらを利用したほうが良いと思います．
kyo0221
commented
2 weeks ago 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の型番が違うからだと考えています.
kyo0221
commented
2 weeks ago 原因が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補正は掛かっているように思える結果になった.
TakumiIshiguro
commented
2 weeks ago @kyo0221 実験内容について訂正で、手押しではなくコントローラー操作で行いました（おそらく影響はないと思いますが）
kyo0221
commented
2 weeks ago yasuohayashibara
commented
2 weeks ago @kyo0221 購入記録が残っている型番は，ADIS16470ではなくADIS16475となります． おそらくプロトコルの違いはないと思いますが，念のためお伝えいたします．
ros2でキャリブレーションできるようにします
https://github.com/technoroad/ADI_IMU_TR_Driver_ROS2 box2で使用しているキャリブレーションのros2バージョンを見つけたのでこれでできるようになるかもしれません.
と思ったのですがよくみたら対応しているIMUのバージョンが一個上の物からでした（box2のIMUは対応している） 実際に使用してみて判断しますが, 新しいの買ってもらうかbox2のIMUと交換することが必要かもしれません.