Oculus Goの姿勢を使ってサーボモーターをコントロールする

[前回の記事](https://qiita.com/hiro-han/items/5303ac758d798f1db587)でOculus Goの姿勢をRaspberry Piに送信する方法を説明しました。
ここではその姿勢情報を使って、Oculus Goの向きを変えたらサーボモーターを回転させるアプリの実装方法を説明します。
処理の流れは以下になります。

*Oculus Go -> 姿勢(クォータニオン) -> Raspberry Pi -> 姿勢(オイラー角) -> Arduino -> サーボモーター*

# デバイス・環境一覧
- Oculus Go
  - Unity 2017.4.1
  - ROS# version 1.2c
- Raspberry Pi4 Model B
  - Ubuntu Server 18.04
  - ROS melodic
- Arduino Uno
- サーボモータ SG-90

# Oculus Goの姿勢を保存する
動作確認のたびにOculus Goを起動するのは手間なので、Oculus Goから出力される姿勢情報を保存しておき、それを再生することで動作確認できるようにします。

[前回の記事](https://qiita.com/hiro-han/items/5303ac758d798f1db587)の通りにアプリを起動します。正しく動作していれば`/oculus_pose`というデータがあるはずです。

```
$ rostopic list
/oculus_pose
...
```
データの保存を開始します。Oculus Goの向きを上下左右に動かしておくと、後で動作確認がやりやすくなります。

```
$ rosbag record /oculus_pose
```
`rosbag`の使い方については以下が参考になります。  

- http://wiki.ros.org/ja/rosbag
- https://qiita.com/srs/items/f6e2c36996e34bcc4d73

またここでOculus Goから出力されるデータ型を確認しておきます。

```
$ rostopic type /oculus_pose
geometry_msgs/PoseStamped
```

# デバイスの接続
## サーボモーターの接続
サーボモータの配線はそれぞれArduinoのピンに接続します
- 黄色 -> 11 pin
- オレンジ -> 5V
- 茶色 -> GND

## RaspiとArduinoの接続
RaspiとArduinoとUSBケーブルで接続します

## /dev/ttyACM0の書き込み権限の付与
通常、一般ユーザでは`/dev/ttyACM0`に対して書き込みができません。`sudo chmod 666 /dev/ttyACM0`などとしてもいいですが、Arduinoを接続する、あるいはRaspberryPiを起動するたびにこのコマンドを実行する必要があります。それは手間なので、一般ユーザに対して書き込み権限を付与します。

```
$ sudo gpasswd --add $USER dialout
```


# Oculus Goから姿勢を受信し、座標変換してArduinoに送信するパッケージを実装する
## 座標変換
### 座標系
Oculus Goから出力される姿勢はクォータニオンです。一方でサーボモータの向きは角度で指定するのでオイラー角が便利です。そのためクォータニオンからオイラー角への変換をします。
さらにOculus GoのUnityとROSでは座標系が異なるため、座標変換が必要です。
クォータニオンが何かについては、検索すればたくさん出てくるため説明しません。

UnityとROSの座標は、それぞれ以下のようになっています。([参考](https://kato-robotics.hatenablog.com/entry/2018/10/24/195041))
- Unity : 左手系
- ROS : 右手系

Unityの値をROSの座標系に変換する方法は以下になります。

- 座標
  - ROS(x, y, z) = Unity(z, -x, y)
- クォータニオン
  - ROS(x, y, z,w) = Unity(z, -x, y, -w)  

### クォータニオン->オイラー角の変換
この変換はROSのライブラリを使えば簡単にできます。

```
double r,p,y; // オイラー角 roll, pitch, yaw
tf::Quaternion quat(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
tf::Matrix3x3(quat).getRPY(r, p, y);
```

## 実装
以下で実装するソースコードはgithubで公開しています。
- [出力メッセージ](https://github.com/hiro-han/ar-car-car-control-msgs.git)
- [座標変換](https://github.com/hiro-han/ar-car-camera-controller.git)

メッセージやパッケージの実装方法の詳細は、[[ROSで簡単なc++パッケージを作成する(初心者向け)](https://qiita.com/hiro-han/items/6f76ea77b5f1388b7207)]や他の方の解説記事を参照してください。

### メッセージ実装
カメラの向きを送信するのに使いやすいメッセージがないため実装します。

```CameraControl.msg
Header header
float32 roll
float32 pitch
float32 yaw
```

### 座標変換パッケージの実装
```camera_controller.cpp
#include <math.h>
#include "ros/ros.h"
#include "car_control_msgs/CameraControl.h"
#include "std_msgs/Header.h"
#include <tf/transform_broadcaster.h>

ros::Publisher publisher;

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;
    ROS_DEBUG("publish :  seq = [%d], r = %f, p = %f, y = %f", msg.header.seq, msg.roll, msg.pitch, msg.yaw);
    publisher.publish(msg);
}

void chatterCallback(const geometry_msgs::PoseStamped &msg) {
  geometry_msgs::Quaternion quaternion = msg.pose.orientation;
  double r,p,y;
  tf::Quaternion quat(quaternion.z, -quaternion.x, quaternion.y, -quaternion.w);
  tf::Matrix3x3(quat).getRPY(r, p, y);
  ROS_DEBUG("convert : seq = [%d], r = %f, p = %f, y = %f", msg.header.seq, r * 180 / M_PI, p * 180 / M_PI, y * 180 / M_PI);
  publishRPY(msg.header, r * 180 / M_PI, p * 180 / M_PI, y * 180 / M_PI);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "camera_controller");
  ros::NodeHandle n;
  publisher = n.advertise<car_control_msgs::CameraControl>("camera_rpy", 100);
  ros::Subscriber subscriber = n.subscribe("oculus_pose", 50, chatterCallback);
  ros::spin();
  return 0;
}
```

## 動作確認
デバッグレベルのログを出力

```
$ rosservice call /basic_logger/set_logger_level {"logger: 'ros', level: 'debug'"}
```

座標変換パッケージの実行

```
$ rosrun camera_controller camera_controller_node
```

保存していたOculus Goの姿勢の再生（ファイル名は環境に応じて変更してください）

```
$ rosbag play (YYYY-MM-DD-hh-mm-ss).bag
```

以下のような結果が表示されれば成功です

```
[ DEBUG] [1603989877.642862698]: convert : seq = [56], r = -60.853410, p = 0.308272, y = -0.562204
[ DEBUG] [1603989877.650206198]: publish :  seq = [56], r = -60.853410, p = 0.308272, y = -0.562204
[ DEBUG] [1603989877.715480515]: convert : seq = [57], r = -61.873506, p = 0.228963, y = -0.435895
[ DEBUG] [1603989877.715763693]: publish :  seq = [57], r = -61.873506, p = 0.228963, y = -0.435895
...
```

# Raspberry Piから姿勢を受信して、サーボモータをコントロールするパッケージを実装する 
これはArduino上で動作するアプリになります。このアプリの実装にはArduino上でROSのノードを動かせる***rosserial***を使用します。
*roseserial*の使い方、それを使ったアプリの実装方法は調べればいくつか出てきますが、ほとんどがArduino IDEを使って実装、ビルドする方法について説明しています。
しかしArduino IDEを使うとIDEを立ち上げる必要があり、実装・ビルドに要する手間がかかるため、ここでは他のROSパッケージ同様、`catkin_make`を使ってコマンドラインでビルドします。[公式](http://wiki.ros.org/rosserial_arduino/Tutorials/CMake)の手順に従って実装します。

ソースコードは[こちら](https://github.com/hiro-han/ar-car-arduino-controller)で公開しています。

## rosserialのインストール
```
$ sudo apt install -y ros-melodic-rosserial-arduino ros-melodic-rosserial
```

## パッケージの生成
```
$ catkin_create_pkg arduino_controller rosserial_arduino rosserial_client std_msgs geometry_msgs
```

## CMakeLists.txtの作成
パッケージフォルダの直下に`CMakeLists.txt`が生成されていますが、下記の内容で上書きします。もしくは、該当箇所を下記に合わせて変更します。

```CMakeLists.txt.txt
cmake_minimum_required(VERSION 2.8.3)
project(arduino_controller)

find_package(catkin REQUIRED COMPONENTS
  rosserial_arduino
  rosserial_client
)

catkin_package()

rosserial_generate_ros_lib(
  PACKAGE rosserial_arduino
  SCRIPT make_libraries.py
)

rosserial_configure_client(
  DIRECTORY firmware
  TOOLCHAIN_FILE ${ROSSERIAL_ARDUINO_TOOLCHAIN}
)

rosserial_add_client_target(firmware arduino_controller ALL)
rosserial_add_client_target(firmware arduino_controller-upload)
```

## 2つめのCMakeLists.txtの作成
パッケージフォルダの直下に`firmware`というフォルダを作り、その中に`CMakefile.txt`というファイルを作成して、下記の内容で保存します。

```firmware/CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)

include_directories(${ROS_LIB_DIR})

# Remove this if using an Arduino without native USB (eg, other than Leonardo)
add_definitions(-DUSB_CON)

generate_arduino_firmware(arduino_controller
  SRCS car_controller.cpp ${ROS_LIB_DIR}/time.cpp
  BOARD uno
  PORT /dev/ttyACM0
)
```
ここで2つ注意点があります。  

- BOARD
  - 自分はArduino Unoを使うため`uno`としています。他のArduinoデバイスを使う場合は変更してください
- PORT  
  - 自分の環境ではRaspberry PiにArduinoを接続すると`/dev/ttyACM0`となりましたが、環境によっては変わる場合があります。その場合は、環境に合わせて変更してください

## ソースコード
```
#include <ros.h>
#include <Arduino.h>
#include <Servo.h>
#include "car_control_msgs/CameraControl.h"

const int CAMERA_PIN = 11;
const int CAMERA_OFFSET = 90; // [degree]

ros::NodeHandle nh;
Servo servo_camera;

void cameraRpyCallback(const car_control_msgs::CameraControl &msg) {
  servo_camera.write(msg.roll + CAMERA_OFFSET);
}

ros::Subscriber<car_control_msgs::CameraControl> sub_camera("/camera_rpy", &cameraRpyCallback);

void setup() {
  servo_camera.attach(CAMERA_PIN);
  servo_camera.write(CAMERA_OFFSET);

  nh.initNode();
  nh.subscribe(sub_camera);
}

void loop() {
  nh.spinOnce();
  delay(1);
}
```

## ビルドおよび、Arduinoへの書き込み
```
$ catkin_make car_controller_arduino_firmware_car_controller
$ catkin_make car_controller_arduino_firmware_car_controller-upload
```

# 動作確認
以下のlaunchファイルを作成して実行します

```test.launch
<launch>
  <node name="camera_controller" pkg="camera_controller" type="camera_controller_node" />
  <node name="arduino_controller" pkg="rosserial_python" type="serial_node.py">
    <param name="port" value="/dev/ttyACM0" />
  </node>
</launch>
```

```
$ roslaunch test.launch
```

別コンソールで保存データの再生

```
$ rosbag play (YYYY-MM-DD-hh-mm-ss).bag
```

サーボモータが回転したら成功です。


# 参考
- ROS実装
  - [[ROSで簡単なc++パッケージを作成する(初心者向け)](https://qiita.com/hiro-han/items/6f76ea77b5f1388b7207)]
- ROS Bag
  - [ROS bag](http://wiki.ros.org/ja/rosbag)
  - [ROS講座29 rosbagを使う](https://qiita.com/srs/items/f6e2c36996e34bcc4d73)
- ROS Serial
  - [ROS serial](https://wiki.ros.org/rosserial_arduino/Tutorials/Blink)
  - [ArduinoにROSのノードをのせる](http://ishi.main.jp/ros/ros_arduino.html)
- クォータニオン
  - [ロボットの位置推定におけるクォータニオン](https://myenigma.hatenablog.com/entry/20130719/1374304154)
  - [ROS と Unity における座標系の違いについて](https://kato-robotics.hatenablog.com/entry/2018/10/24/195041)

# リンク
ARラジコンを開発する記事の一覧

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