ARラジコンをOculusQuest2で操作する

[以前作成したラジコン](https://qiita.com/hiro-han/items/b2e5ae68d16e9116b37e)では、OculusGoでカメラ画像表示、Nintendo Switchのプロコンで操作をしていましたが、Oculus Quest 2を手に入れたので、画像表示・操作をOculus Quest 2でできるようにしました。

# 環境
- Oculus Quest 2
  - Unity 2019.4.15f1 LTS
  - ROS# 1.6
- Raspberry Pi 4
  - ROS melodic

# 実装
今回、以前作成したソースコードに変更を加えました。
全てをこの記事内で説明すると長くなるので、変更部分だけを説明しています。
全体については、元記事をご参照ください。

- [Oculus GoにRaspiのカメラ画像を表示する&姿勢を取得する](https://qiita.com/hiro-han/items/5303ac758d798f1db587)
- [Oculus Goの姿勢を使ってサーボモーターをコントロールする](https://qiita.com/hiro-han/items/01561be3bfdc484a8407)
- [Raspberry Pi, Arduinoでタミヤラジコンを制御する](https://qiita.com/hiro-han/items/86b2ef8aa589c9f916f8)

## Oculus Quest 2側の実装
[別の記事「Oculus Quest 2でROS Sharpを使う」](https://qiita.com/hiro-han/items/a36c54e869d2a199402d)にまとめました。
この記事内で、ROSから受信したカメラ画像の表示・ROSへのヘッドセットの向き出力・ROSへのコントローラの出力を説明しているので、ご参照ください。ざっくりとは以下になります。

ソースコードはこの[GitHub](https://github.com/hiro-han/ar-car-oculus-quest2)で公開しています。

- カメラ画像表示
    - ROS#の「ImageReceiver」を使用
    - 受信topic名を「/image_raw/compressed」に設定
- ヘッドセットの向き出力
    - 「CenterEyeAnchor」のTransformを出力
    - ROS#の「PoseStampedPublisher」を使用
    - 出力topic名を「/oculus/head_set/pose」に設定
- コントローラ出力
    - 左右のJoyスティックを出力
    - ROS#の「JoyPublisher」を使用
    - 出力topic名を「/oculus/controller」に設定

## Rasphberry Piの実装
## カメラ表示
「[Oculus GoにRaspiのカメラ画像を表示する&姿勢を取得する](https://qiita.com/hiro-han/items/5303ac758d798f1db587)」と変更はありません。
ROSの`uvc_camera`パッケージと`image_transport`パッケージをそのまま使用しています。

## カメラ向き操作
「[Oculus Goの姿勢を使ってサーボモーターをコントロールする](https://qiita.com/hiro-han/items/01561be3bfdc484a8407)」で作成した`camera_controller`パッケージに変更を加えました。
ヘッドセットの向きの正負が以前と逆になったようなので、正負を逆にする処理を追加しました。


``` camera_controller.cpp 
void publishRPY(const std_msgs::Header &header, const double r, const double p, const double y) {
    car_control_msgs::CameraControl msg;
    msg.header = header;
    msg.roll = r;
    msg.pitch = p;
    msg.yaw = y;
    // 追加ここから
    if (type == HeadSetType::OculusQuest2) {
      msg.roll = -msg.roll;
    }
    // 追加ここまで
    ROS_DEBUG("publish :  seq = [%d], r = %f, p = %f, y = %f", msg.header.seq, msg.roll, msg.pitch, msg.yaw);
    publisher.publish(msg);
}
```

その他は受信するデバイス種別に`OculusQuest2`という定義を追加したことと、デバイス種別に応じて受信するtopic名を切り替えるようにしました（[差分](https://github.com/hiro-han/ar-car-camera-controller/commit/68fe85910383152b969c5e2d5f9c845ed6e613db)）。
ソースコードは[こちらのGitHub](https://github.com/hiro-han/ar-car-camera-controller/releases/tag/v1.1)です。


## ラジコン操作
「[Raspberry Pi, Arduinoでタミヤラジコンを制御する](https://qiita.com/hiro-han/items/86b2ef8aa589c9f916f8)」で作成した`car_controller`パッケージに変更を加えました。
ジョイスティックの左右の正負が、Nintendo Switchのプロコンと逆（OculusQuest2は右が正だが、プロコンは左が正）なので、OculusQuest2のときは正負を逆にしました。

``` car_contlloer.cpp

void joyCallback(const sensor_msgs::Joy& msg) {
  car_control_msgs::RcCarControl control;
  switch (type) {
  case ControllerType::ProCon:
    control.steer = msg.axes[0];
    control.speed = msg.axes[3];
    break;
  case ControllerType::PS3:
    control.steer = msg.axes[0];
    control.speed = msg.axes[4];
    break;
  // 追加ここから
  case ControllerType::OculusQuest2:
    control.steer = -msg.axes[0];
    control.speed = msg.axes[3];
    break;
  // 追加ここまで
  default:
    control.steer = msg.axes[0];
    control.speed = msg.axes[3];
    break;
  }
  ROS_DEBUG("axis 0 = [%.3f], 4 = [%.3f]", control.steer, control.speed);
  publishCarControl(control);
}
```

その他はカメラ向き操作と同じように、受信するデバイス種別に`OculusQuest2`という定義を追加したことと、デバイス種別に応じて受信するtopic名を切り替えるようにしました（[差分](https://github.com/hiro-han/ar-car-car-controller/commit/cee23ad9d5567d892a49c169dea9af4e6f4700b9)）。
ソースコードは[こちらのGitHub](https://github.com/hiro-han/ar-car-car-controller/releases/tag/v1.1)です。


## ビルド

```
$ cd {ROS Workspace}
$ catkin_make --pkg camera_contlloer
$ catkin_make --pkg car_contlloer
```

# 実行
ROS側は以下のlaunchファイルを作成し、実行しました。

``` .xml
<launch>
  <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" /> 
  <param name="rosbridge_websocket/address" value="192.168.4.8"/>
  <node name="uvc_camera" pkg="uvc_camera" type="uvc_camera_node">
    <param name="width" type="int" value="1280"/>
    <param name="height" type="int" value="720"/>
  </node>
  <node name="camera_controller" pkg="camera_controller" type="camera_controller_node">
    <param name="headset_type" value="OculusQuest2" type="string" />
  </node>
  <node name="car_controller" pkg="car_controller" type="car_controller_node">
    <param name="controller_type" value="OculusQuest2" type="string" />
  </node>
  <node name="arduino_controller" pkg="rosserial_python" type="serial_node.py">
    <param name="port" value="/dev/ttyACM0" />
  </node>
</launch>
```

# 参考
記事内でもリンクを貼った、ARラジコンを作成する記事です
- [ARラジコンを作る](https://qiita.com/hiro-han/items/b2e5ae68d16e9116b37e) (まとめの記事)
- [Oculus GoにRaspiのカメラ画像を表示する&姿勢を取得する](https://qiita.com/hiro-han/items/5303ac758d798f1db587)
- [Oculus Goの姿勢を使ってサーボモーターをコントロールする](https://qiita.com/hiro-han/items/01561be3bfdc484a8407)
- [Raspberry Pi, Arduinoでタミヤラジコンを制御する](https://qiita.com/hiro-han/items/86b2ef8aa589c9f916f8)
- [Raspberry PiでNintendo Switchのプロコンを使う](https://qiita.com/hiro-han/items/ac460a554529e1764171)
- [Oculus Quest 2でROS Sharpを使う](https://qiita.com/hiro-han/items/a36c54e869d2a199402d)