構造・命名規則のリファクタリング

[kyo0221]

概要

• 命名規則やパッケージのまとめ方について，整備する
• planningやcontrolなど，複数のパッケージにまたがる要素を持つディレクトリは，パッケージを内包させる
  • 例えば，e2e_plannerはplanningに含める
  • controlには手動操縦用のcontrollerや，経路追従用のtrajectory_followerが含まれる
• マップベースナビゲーションの処理や実装を見直す（改良）
• その他マップベースのナビゲーションの機能拡張を行いたい（改良）
  • 走行ルートを増やし，逆走含め，コース全域を走行できるようにしたい

※改良に関してPRを細分化したほうが良ければそうするので，意見をください

ToDo

• rename
• パッケージ再配置
• 処理・実装の見直し
  • map
  • localization
  • planner
    • mission_planner
    • local_planner
  • control
• 機能拡張

[kyo0221]

trajectory followerにPidMpcPlugin追加

• 並進方向の速度制御にPID，横方向の経路追従にMPCを持ちいる制御
  • 曲率に応じて速度を可変にできる
  • v = sqrt(a_lat_max / |κ|)から速度制限を導出
    • a_lat_max : 横方向加速度最大値（parameter）
    • κ：曲率
  • 運動学モデルには二輪モデルを使っています（高速になるほど誤差は出るが）

[kyo0221]

カメラ行列の値を調整し，vectormapに対してそれなりに位置推定の正確さを発揮できるようになった

bagデータを基に，オープンループの環境で検証

[kyo0221]

localizationについて

ekfの実装で，状態を[x,y,yaw]にしているが進行方向に対して舵角が支配的になっている関係で，状態に舵角を加えることを検討中

運動学モデル

x_{k+1}   = x_k + v・cosθ_k・Δt
y_{k+1}   = y_k + v・sinθ_k・Δt
yaw_{k+1} = yaw_k + ω・Δt