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

前回の記事で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から出力される姿勢情報を保存しておき、それを再生することで動作確認できるようにします。

前回の記事の通りにアプリを起動します。正しく動作していれば/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の座標は、それぞれ以下のようになっています。(参考)

• 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で公開しています。

• 出力メッセージ
• 座標変換

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

メッセージ実装

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

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を使ってコマンドラインでビルドします。公式の手順に従って実装します。

ソースコードはこちらで公開しています。

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++パッケージを作成する(初心者向け)]
• ROS Bag
  • ROS bag
  • ROS講座29 rosbagを使う
• ROS Serial
  • ROS serial
  • ArduinoにROSのノードをのせる
• クォータニオン
  • ロボットの位置推定におけるクォータニオン
  • ROS と Unity における座標系の違いについて

リンク

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

1. ARラジコンを作る (概要)
2. Oculus GoにRaspiのカメラ画像を表示する&姿勢を取得する
3. Oculus Goの姿勢を使ってサーボモーターをコントロールする] (本記事)
4. Raspberry Pi, Arduinoでタミヤラジコンを制御する