コントローラーの入力をAWSIMに渡すROS2ノードを自作して手動運転AIチャレンジしてみた

# はじめに
自動運転AIチャレンジに参加したものの、自作のプログラムを実装するにはどうしたらいいか分からない、という状態だった筆者が初めてROS2ノードの作成をした記録です。筆者はもともとAssetto CorsaやGran Turismo 7といったドライビングシミュレータで遊ぶためにハンドルコントローラーを所有していたので、これで自動運転AIチャレンジ2023シミュレーション大会で提供されているAWSIM内のレーシングカーを操作できたら面白いと思い、そのために必要なノードを実装しました。

# 前提
自動運転AIチャレンジの舞台であるAWSIMとAutowareはROS2をミドルウェアとして開発されています。ROS2ではソフトウェアはノードの集合体として動作しており、各ノードはトピックを介して通信しています。トピックにデータを送信することを __publish__ と呼び、トピックからデータを受信することを __subscribe__ と呼びます。ROSノードを実装する際にはどのトピックをsubscribeしてどのトピックにpublishすればよいのかと、各トピックのメッセージの中身を特定する必要があります。

# 方針
AWSIMが車両制御情報を受信するためにsubscribeしているトピックを特定し、ゲームコントローラーの入力を適切に処理した上でそのトピックにpublishするノードを実装することにします。

### 出力側
そこでAWSIMがどのトピックから制御情報を受け取っているのか確かめるために、AWSIMのみを起動した状態で存在しているトピックとそこに流れているメッセージの型を確認してみます。
```
$ ros2 topic list -t
/awsim/ground_truth/localization/kinematic_state [nav_msgs/msg/Odometry]
/awsim/ground_truth/perception/object_recognition/detection/objects [autoware_auto_perception_msgs/msg/DetectedObjects]
/clock [rosgraph_msgs/msg/Clock]
/from_can_bus [can_msgs/msg/Frame]
/parameter_events [rcl_interfaces/msg/ParameterEvent]
/powertrain_data [autonoma_msgs/msg/PowertrainData]
/race_control [autonoma_msgs/msg/RaceControl]
/rosout [rcl_interfaces/msg/Log]
/to_can_bus [can_msgs/msg/Frame]
/to_raptor [autonoma_msgs/msg/ToRaptor]
/vehicle_data [autonoma_msgs/msg/VehicleData]
/vehicle_inputs [autonoma_msgs/msg/VehicleInputs]
```
これを見ると、```/vehicle_inputs```というトピックがあることが分かります。今回のAWSIMはこのトピックから得たデータを入力として車両を走らせていそうです。
このトピックに流れるメッセージの型```autonoma_msgs/msg/VehicleInputs```の詳細を確認してみます。(この出力は```autonoma_msgs```がbuildされてsourceされていないと得られません。)
```
$ ros2 interface show autonoma_msgs/msg/VehicleInputs 
std_msgs/Header                 header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id

# Throttle command (%)
float32 throttle_cmd
uint8 throttle_cmd_count

# Brake pressure command (kPa)
float32 brake_cmd
uint8 brake_cmd_count

# Steering motor angle command (degrees)
float32 steering_cmd
uint8 steering_cmd_count

# Gear command
uint8 gear_cmd
```
送るべき情報はアクセル開度(%)、ブレーキ踏力(kPa)、ステアリング角度(deg)、ギアポジションと分かります。

### 入力側
ROS2でゲームコントローラーを扱う方法については、USB接続したコントローラーの入力内容をトピック```/joy```にpublishするノードであるパッケージ[joy](https://index.ros.org/p/joy/)があるのでこれを使います。
トピック```/joy```のメッセージ型は```sensor_msgs/msg/Joy```で、その内容は以下です。
```
$ ros2 interface show sensor_msgs/msg/Joy
# Reports the state of a joystick's axes and buttons.

# The timestamp is the time at which data is received from the joystick.
std_msgs/Header header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id

# The axes measurements from a joystick.
float32[] axes

# The buttons measurements from a joystick.
int32[] buttons
```
アナログ入力は配列```axes```に-1.0 ~ 1.0の小数で、ボタン入力は配列```buttons```に0 or 1で入ります。
コントローラーを接続して、1つ目のターミナルで
```
$ ros2 run joy joy_node
```
2つ目のターミナルで
```
$ ros2 topic echo /joy
```
を実行し、コントローラーを操作してみると値が変わるのが分かります。各ボタンが配列の何番目の値に対応しているかメモしておきます。

# 実装
目標は、トピック```/joy```をsubscribeし、それに処理をくわえてトピック```/vehicle_inputs```にpublishするノードを作ることです。

publishとsubscribeの方法についてはChatGPTに教えてもらいました。

https://chat.openai.com/share/3960c9db-f7dd-4d96-8a6a-34ebf3929f61

受け取った入力に対し以下の処理をして、出力するメッセージを生成しています:
- アクセル入力を-1.0(入力なし) ~ 1.0(入力最大)から0 ~ 100(%)へ線形変換
- ブレーキ入力を-1.0(入力なし) ~ 1.0(入力最大)から0 ~ 3000(kPa)へ線形変換(3000という値はAWSIMのブレーキゲージを見ながら決めました)
- ステアリング入力を-1.0(右最大) ~ 1.0(左最大)から-450 ~ 450(deg)へ線形変換(今回使用したハンドルコントローラーの最大舵角と一致させるための設定です)
- シフトアップボタンが押された瞬間を検知し、シフトポジションが5以下なら1増やす
- シフトダウンボタンが押された瞬間を検知し、シフトポジションが2以上なら1減らす

また、複数のコントローラーを扱えるように、入力配列のindexと各ボタンの対応をyamlファイルに記述しておき、ROS2 parameterとして設定するようにしてあります。デフォルトのThrustmaster T150RSというハンドルコントローラー用の設定に加え、PS5のコントローラー用の設定も作成しました。(なお、この2つのコントローラーではアクセルとブレーキの入力の正負が反転していたため、PS5の場合に入力を-1倍する処理をしています。最大舵角の変更もするべきですが、未対応です。)

最後に、```CMakeLists.txt```と```package.xml```に依存関係の記述を忘れずにしておきます(```/joy```を扱うために```sensor_msgs```が、```/vehicle_inputs```を扱うために```autonoma_msgs```が必要です)。

作成したパッケージはGitHubに公開しています。

https://github.com/ChikaraMatsuzawa/gamepad_controller_cpp

# 使い方
AIチャレンジのdockerコンテナの外で使うことを想定しています。
1. ```~/ros2_ws/src```に```gamepad_controller_cpp```と```aichallenge_ws/src```から持ってきた```sim-msgs```を置く
2. ```~/ros2_ws```で```colcon build```と```source install/setup.bash```
3. ```ros2 run gamepad_controller_cpp gamepad_controller_cpp_node```
4. 別のターミナルで```ros2 run joy joy_node```

```ros2 run gamepad_controller_cpp gamepad_controller_cpp_node --ros-args --params-file ~/ros2_ws/src/gamepad_controller_cpp/config/dualsense.yaml```とすることでPS5用コントローラーで操作可能

# 使ってみた

作成したノードを使ってハンドルコントローラーによる手動運転でSimulation大会のコースを一周してみたときの画面キャプチャとrosbagが以下です。

<iframe width="560" height="315" src="https://www.youtube.com/embed/ld3vSvlSPAM?si=a6nd8y8eExIe78X_" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

https://drive.google.com/drive/folders/1iIuO_wEG48iv_K8exIgqzYDdsorMYpEP?usp=sharing

自分で運転してみて以下のようなことを感じました:
- ステアリングは思った以上に早く切り始めないと切り遅れる
- 1速や2速で4000回転くらいまで回すと簡単にスピンする
- 強いブレーキをかけてタイヤがロックすると減速も旋回もほとんどできなくなる(のでABSのような制御が欲しい？)

# さいごに
ROS2ノードの実装についてどこから始めたらよいか分からない状態でしたが、手探りながら実装してみてsubscribeとpublishさえできれば後は実現したい処理の実装に集中できそうだと分かりました。今大会ではパラメータ調整だけでなく自作モジュールを組み込んでみたいところです。手動走行ができるとデータも取りやすくなり役に立ちそうです。