LoginSignup
2
3

More than 1 year has passed since last update.

@hiro-han

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

Last updated at Posted at 2020-12-11

前回の記事で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の使い方については以下が参考になります。

またここで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

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

参考

リンク

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

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

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
3