search

open-rdc / aiformula

MIT License
5 stars 1 forks source link

経路追従のシミュレータ環境での実験 #98

Open kyo0221 opened 3 months ago

kyo0221 commented 3 months ago

経路追従の挙動を確認するためにシミュレータ環境で動作させる.

シミュレータ環境は送られてきたフォーミュラから吸い出したリポジトリに入っていたものを使用している. 現状のmain_executorを実行するとcanのエラーがでてしまうため, 以下の記事を参考に仮想CANインターフェイスを使用する. https://qiita.com/bunnyhopper_isolated/items/33a7b7d6a7ceeb51323d

シミュレータ環境で/vectornav/poseを再現するために, gazeboから出ている/odomからpositionとorientationを抜き取り, positionにはオフセットをかけてorientationをそのまま使用した/vectornav/poseをパブリッシュするスクリプトを作成して使用している. https://github.com/kyo0221/script

ブランチはfeat/gnssnav_developを使用している.

kyo0221 commented 3 months ago

シミュレータ環境で実験するにあたり, cmd_velのメッセージ型をgazebo側と統一する必要が合ったため, Twistで統一させた.

現状では前述の条件でmain_executorを立ち上げると以下のようなエラーが出て処理が止まってしまう. [main_exec-2] terminate called after throwing an instance of 'rclcpp::exceptions::RCLBadAlloc' [main_exec-2] what(): std::bad_alloc [ERROR] [main_exec-2]: process has died [pid 7212, exit code -6, cmd '/home/kyo/formula_ws/install/main_executor/lib/main_executor/main_exec --ros-args --params-file /home/kyo/formula_ws/install/main_executor/share/main_executor/config/main_params.yaml'].

実機ではこのエラーが発生することがほとんどないため, シミュレータ環境特有の問題となっている.

kyo0221 commented 3 months ago

/vectornav/poseの座標変換(ECEF→UTM)でエラーが発生している様子だったので, /vectornav/poseをシミュレータで使用することを諦めて/odomを直接使用することにした.

現状では進んでいる方向は違うが, 目標点に向かって追従するように動く様子が確認できている. しかし, ある目標点の次の目標点を定めたときにエラーが発生してしまう.

進んでいる方向が違うのはシミュレータ上での北と実際の紫峰での北にズレがあるからだと思われる.

kyo0221 commented 3 months ago

経路追従のシミュレーションをする際にcsvファイルはshihou_full.csvを使用していたが, gazebo_shihou_cource.csvとはスタート地点が異なっているなどの違いがあることがわかった.

前述の進んでいる方向が違う問題はcsvファイルが違うことを考えたら正常な向きに進んでいたため, 目標点のとり方が合っていることが確認できた.

しかし, csvファイルをgazebo_shihou_cource.csvに変更して実行すると次のようなエラーが発生する. [main_exec-2] main_exec: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:408: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator [with Derived = Eigen::Array<double, 1, -1, 1, 1, -1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]: Assertion `index >= 0 && index < size()' failed.

gazebo用のcsvファイルは他のcsvファイルと比べてかなり細かく座標指定されているので, 格納できる範囲外の値にアクセスしてしまっている可能性があると思われる.

kyo0221 commented 2 months ago

既存のステアリング制御は目標点との相対角度をそのまま比例制御に使用していたが, 以下の記事のステアリング制御を参考にして変更し, 動作させた. https://qiita.com/porizou1/items/a727169fa656b3a5d15e

https://youtu.be/dPiY28NR9ps

目標地点に対して追従する様子は確認できたが, rviz2での目標地点とロボットの位置を見ると目標地点にロボットが近づいても目標地点が更新されていない様子が確認できる.