learning_based_vehicle_calibrationを使ってみよう

# はじめに

自動運転AIチャレンジ2024決勝まで、残り2週間を切りました。
参加者の皆様、開発は順調に進んでおりますでしょうか…？

https://www.jsae.or.jp/jaaic/2024ver/summary/

さて、前回の記事では`accel_brake_map_calibrator`を使って、
`accel_map.csv` / `brake_map.csv` のオフラインキャリブレーションを試しました。

https://qiita.com/soyaoki/items/52e93d14d7c3e4243cb3

残りの`steer_map.csv`はどのように調整するのかな〜と迷っていたところ、以下`learning_based_vehicle_calibration`を見つけました。

https://github.com/orgs/autowarefoundation/discussions/4663

https://github.com/pixmoving-moveit/learning_based_vehicle_calibration

ニューラルネットワーク(3層MLP, MSELoss)で該当マップを学習させており、
システムの非線形性をうまく捉えることができるようです。

なんだかすごそう…ということでさっそく使ってみました。

:::note warn

先に結論です。この方法で **簡単に競技成績が良くなることはない** と思います。

機械学習を使ったキャリブレーション自体は動いて、
マップCSVファイルも作成されるのですが、これを使って自動運転ができません。

具体的には、以下のようにステアリング指令値がバタついて走りません。
そしてこれは、デフォルトで提供されている`steer_map.csv`も同様のようです。
そもそもマップを使ったステア補正は現状OFFされているので非推奨なのかもしれません。

![ezgif-2-7da60e5819.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/18383185-a954-2008-a5ee-2996549b3979.gif)

従って、`steer_map.csv`は使わない方がベター…と思います。
(本気でやったら効果はあるのかもしれませんが、沼な気配を感じています…)

:::

# `learning_based_vehicle_calibration`の使い方

**縦方向ダイナミクスキャリブレーション** (`accel_map.csv` / `brake_map.csv`作成) と
**ステアリングダイナミクスキャリブレーション** (`steer_map.csv`作成) がありますが、
今回はステアリングダイナミクスのみ試してみます。

:::note info
以降出てくるコマンドは、すべてdocker開発環境内で実行してください。
:::

## ステアリングダイナミクスキャリブレーション

前回同様、運営様から共有されたbagファイルを使ったオフラインキャリブレーションを試してみます。


### 1. 事前準備

まずは以下を`aichallenge-2024/aichallenge/workspace/src/aichallenge_submit`下にクローンします。基本はこのREADME.mdに沿って実行すれば動きます。ありがたや。

https://github.com/pixmoving-moveit/learning_based_vehicle_calibration/tree/normal_vehicle

コマンドはこちら。

```bash:Terminal 1
cd /aichallenge/workspace/src/aichallenge_submit
git clone https://github.com/pixmoving-moveit/learning_based_vehicle_calibration.git && cd learning_based_vehicle_calibration
```

続いて、依存関係をインストールします。Pytorch/CUDA等、10分ほど時間を要します。

```bash:Terminal 1
pip install -r requirements.txt 
sudo apt install ros-humble-can-msgs -y
```

ビルドします。

```bash:Terminal 1
cd /aichallenge
./build_autoware.bash
```

### 2. 実行

**データ収集** と **モデル学習・キャリブレーションマップ作成**　の2ステップで、
`steer_map.csv`が作成されます。

#### 2.1. データ収集

このステップでは、各種トピックからキャリブレーション用データを取得し、CSVファイルとして保存します。
この処理はノード`data_collection_steer`が担当し、以下の Sub / Pub を用います。

```bash
$ ros2 node info /data_collection_steer
/data_collection_steer
  Subscribers:
    /sensing/gnss/chc/imu: sensor_msgs/msg/Imu
    /sensing/gnss/chc/pitch: std_msgs/msg/Float32
    /vehicle/status/actuation_status: tier4_vehicle_msgs/msg/ActuationStatusStamped
    /vehicle/status/steering_status: autoware_auto_vehicle_msgs/msg/SteeringReport
    /vehicle/status/velocity_status: autoware_auto_vehicle_msgs/msg/VelocityReport
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /scenarios_collection_steering_progress: learning_based_vehicle_calibration/msg/SteeringProcesses
  Service Servers:
    /data_collection_steer/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /data_collection_steer/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /data_collection_steer/get_parameters: rcl_interfaces/srv/GetParameters
    /data_collection_steer/list_parameters: rcl_interfaces/srv/ListParameters
    /data_collection_steer/set_parameters: rcl_interfaces/srv/SetParameters
    /data_collection_steer/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:
```

実行コマンドはこちら。上限車速とスロットル閾値をデフォルトから変えています。

```bash:Terminal 1
source /aichallenge/workspace/install/setup.bash
cd /aichallenge/workspace/src/aichallenge_submit/learning_based_vehicle_calibration
ros2 launch learning_based_vehicle_calibration calibration_steering_launch.py max_velocity:=10.0 throttle_threshold:=50
```

以下のような表示が出たらOKです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/c65acaf3-b660-b500-4184-a440573afd88.png)

次に別ターミナルを開き、bagファイルを再生する以下コマンドを実行します。
`data_collection_steer`に渡せるように、IMUトピックをリネームしておきます。

```bash:Terminal 2
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 bag play /output/0920_demo_interface_only/0920_demo_interface_only_0.db3 --clock --start-offset 415.0 --remap /sensing/imu/imu_raw:=/sensing/gnss/chc/imu
```

一応、pitch情報も渡しておきます。なくても動きそう…

```bash:Terminal 3
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 topic pub /sensing/gnss/chc/pitch std_msgs/msg/Float32 "data: 0.0" -r 10
```

モニタリングスクリプトも実行しておくと吉です。

```bash:Terminal 4
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
python3 /aichallenge/workspace/src/aichallenge_submit/learning_based_vehicle_calibration/scripts_steering/data_monitor_steer.py 
```

キャリブレーションに必要なデータがPubされているときは以下のような表示で、
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/e62bac42-5419-834f-3376-05784914358e.png)

不足していると以下のようにエラーを出してくれます。便利。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/771f3f6e-790f-628a-3bbf-08dd71a2cc01.png)



#### 2.2. モデル学習・キャリブレーションマップ作成

bagファイルの再生が終わると、以下のようにデータが溜まっていると思います。
十分とは言い難いですが、これでキャリブレーションしてみます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/3d876f3f-871c-e6fa-2381-62dfdff14403.png)


まずは、`steering_0x.csv`が`/aichallenge/workspace/src/aichallenge_submit/learning_based_vehicle_calibration`下にあることを確認します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/144225cb-f4cb-7a79-014f-0db352b0c2cc.png)

`Terminal 1`で実行中のlaunchを`Ctrl+C`で停止させ、次いで以下を実行します。
まずはデータの可視化です。

```bash:Terminal 1
python3 scripts_steering/steering_data_visualization.py
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/4c78cb6c-4b37-efc6-eb35-3de479f58335.png)

明らかにデータが不足している感がありますが、いったん目を瞑ります。

次がいよいよ学習とマップ作成です。以下を実行します。
データが少ないので、フィルタ条件を緩和してほぼ全て学習に使用可能とします。

```bash:Terminal 1
ros2 launch learning_based_vehicle_calibration neural_network_steering_launch.py filter_vel:=100.0 filter_cmd:=100.00 filter_acc:=100.0
```

以下のような、フィッティング結果と学習データのヒストグラムが5つ表示されます。
各`steering_0x.csv`に対応付いた学習結果のようです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/ec8ac43e-f0f3-7a5b-3b0f-a0a04502a442.png)

ここまで来れば、`/aichallenge/workspace/src/aichallenge_submit/learning_based_vehicle_calibration`下に`steer_map_x.csv`が作成されていると思います。こちらが最終アウトプットのキャリブレーション用マップとなります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/ecbaf1c8-12f3-9650-4052-6b9ed322af52.png)

デフォルトの`steer_map.csv`との比較がこちら。
※ 図中の Map 6 が デフォルトのマップです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/4e5efb5d-0207-1477-e964-6a5cafc02406.png)

むむ、なんだかスケールが全然違うぞ…と不穏な空気が漂いますが、先に進みます。

### 3. シミュレーション

作成したマップを使って、自動運転シミュレーションをしてみます。

まずは、`steer_map_2.csv`を`/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/`下にコピーします。

>For example, the first calibration table is valid for steering angles between 5% and 20%, the second one is valid for steering angles between 20% and 40% etc.

本来は、ステア角ごとに参照するマップを変えることを想定しているようですが、
簡単にできなさそうなのでマップは固定します。


そして`reference.launch.xml`を以下のように変更します。
ステアリング補正はデフォルトだとOFFになっていますので、ONにしています。

```diff_xml:reference.launch.xml
  <!-- vehicle -->
  <group if="$(var launch_vehicle_interface)">
    <include file="$(find-pkg-share raw_vehicle_cmd_converter)/launch/raw_vehicle_converter.launch.xml">
      <arg name="converter_param_path" value="$(find-pkg-share aichallenge_submit_launch)/config/converter.param.yaml"/>
      <arg name="csv_path_accel_map" value="$(find-pkg-share aichallenge_submit_launch)/data/accel_map.csv"/>
      <arg name="csv_path_brake_map" value="$(find-pkg-share aichallenge_submit_launch)/data/brake_map.csv"/>
+     <arg name="csv_path_steer_map" value="$(find-pkg-share aichallenge_submit_launch)/data/steer_map_2.csv"/>

      <arg name="max_throttle" value="1.0"/>
      <arg name="max_brake" value="1.0"/>
      <arg name="convert_accel_cmd" value="true"/>
      <arg name="convert_brake_cmd" value="true"/>
-     <arg name="convert_steer_cmd" value="false"/>
+     <arg name="convert_steer_cmd" value="true"/>

      <arg name="input_control_cmd" value="/control/command/control_cmd"/>
      <arg name="input_odometry" value="/localization/kinematic_state"/>
      <arg name="input_steering" value="/vehicle/status/steering_status"/>
      <arg name="output_actuation_cmd" value="/control/command/actuation_cmd"/>
    </include>
  </group>
```


`./run_evaluation.bash`でシミュレーションを実行してみると…

…

ステアが暴れまくって、そもそも走れませんでした…😭

![ezgif-2-7da60e5819.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/18383185-a954-2008-a5ee-2996549b3979.gif)

デフォルトのマップ`steer_map.csv`でも同じ挙動でした…😭

正直ここの変更は沼っぽく、現状デフォルト設定で走れているなら手を入れない方が良さそうです…データ量が問題の可能性もあるので、事前練習でのbagファイルを使ってみるのはアリですが、なんとなく難しそうだなあという感想です…

# おわりに

機械学習を使ったステアダイナミクスのキャリブレーションにトライしてみましたが、残念な結果に終わりました…
もう少し深堀りするか悩みますが、他にも重大な問題が山積しているので、優先度は下げようかな…と思います。
来年度以降、これに挑戦する方のために記事を残しておきます… **後は頼みます。**

この記事は、AI文章校正ツール「ちゅらいと」で校正されています。