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の姿勢を保存する
- Unity 2017.4.1
- ROS# version 1.2c
- Ubuntu Server 18.04
- ROS melodic
動作確認のたびに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(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++パッケージを作成する(初心者向け)]や他の方の解説記事を参照してください。
メッセージ実装
カメラの向きを送信するのに使いやすいメッセージがないため実装します。
Header header
float32 roll
float32 pitch
float32 yaw
座標変換パッケージの実装
#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
が生成されていますが、下記の内容で上書きします。もしくは、該当箇所を下記に合わせて変更します。
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
というファイルを作成して、下記の内容で保存します。
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デバイスを使う場合は変更してください
- 自分はArduino Unoを使うため
- PORT
- 自分の環境ではRaspberry PiにArduinoを接続すると
/dev/ttyACM0
となりましたが、環境によっては変わる場合があります。その場合は、環境に合わせて変更してください
- 自分の環境ではRaspberry PiにArduinoを接続すると
ソースコード
#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ファイルを作成して実行します
<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 Bag
- ROS Serial
- クォータニオン
リンク
ARラジコンを開発する記事の一覧
- ARラジコンを作る (概要)
- Oculus GoにRaspiのカメラ画像を表示する&姿勢を取得する
- Oculus Goの姿勢を使ってサーボモーターをコントロールする] (本記事)
- Raspberry Pi, Arduinoでタミヤラジコンを制御する
Author And Source
この問題について(Oculus Goの姿勢を使ってサーボモーターをコントロールする), 我々は、より多くの情報をここで見つけました https://qiita.com/hiro-han/items/01561be3bfdc484a8407著者帰属:元の著者の情報は、元のURLに含まれています。著作権は原作者に属する。
Content is automatically searched and collected through network algorithms . If there is a violation . Please contact us . We will adjust (correct author information ,or delete content ) as soon as possible .