【ROS】DS4コントローラを振動させる | 空飛ぶラズベリーパイ

やりたいこと

ROSを使うとプレステのコントローラ「DualShock4」を使ってロボットの操作が簡単にできます。しかし、コントローラの操作をロボットに伝える事例はたくさんあっても、コントローラを振動させる（フォースフィードバック）やり方でハマったのでご紹介します。

王道は「ds4drv」と「joy_node」を使う。でも…

DS4コントローラとROSの連携で最も一般的なのはコントローラ用のドライバ「ds4drv」とROSのjoyパッケージのノードである「joy_node」を使うやり方です。（こちらのサイトを参考にしました）
ROSでPS4のコントローラ(DualShock4)を使う方法

ところが、振動機能を試してみると失敗します。
以下は失敗事例です。
振動のテストにはfftestコマンドを使います。まずはds4drvを起動して、コントローラとペアリング

$ sudo ds4drv 
[info][controller 1] Created devices /dev/input/js0 (joystick) /dev/input/event11 (evdev) 
[info][bluetooth] Scanning for devices
[info][bluetooth] Found device DC:0C:2D:5C:86:2B
[info][controller 1] Connected to Bluetooth Controller (DC:0C:2D:5C:86:2B)
[info][bluetooth] Scanning for devices
[info][controller 1] Battery: 37%
[warning][controller 1] Signal strength is low (31 reports/s)

一番上の行にあるevdevのイベント番号を見ます。今回は/dev/input/event11となっています。続いてfftestを実行します。

$ fftest /dev/input/event11
Force feedback test program.
HOLD FIRMLY YOUR WHEEL OR JOYSTICK TO PREVENT DAMAGES

Device /dev/input/event11 opened
…
（中略）
…

Uploading effect #0 (Periodic sinusoidal) ... Error:: Function not implemented
Uploading effect #1 (Constant) ... Error: Function not implemented
Uploading effect #2 (Spring) ... Error: Function not implemented
Uploading effect #3 (Damper) ... Error: Function not implemented
Uploading effect #4 (Strong rumble, with heavy motor) ... Error: Function not implemented
Uploading effect #5 (Weak rumble, with light motor) ... Error: Function not implemented
Enter effect number, -1 to exit

Uploading effectで始まる行ですべてError: Function not implementedが出てしまっています。この状態で0~5の数字キーを押しても全く振動してくれません。

確認すべきこと

カーネルのバージョン

まず第一に、linuxカーネルのバージョンを確認しましょう。linuxカーネルとはOSの中核を担うプログラムのことです。ds4の振動機能はlinuxではカーネル4.10以上でしかサポートされていないので、それ未満の場合は更新する必要があります。

$ uname -r 
4.9.0-43-generic

linuxカーネルはapt-get upgrade等では自動更新されないので、手動で更新する必要があります。といってもapt-getを使って簡単に更新できます。(※バージョンによっては更新後にエラーが起こりLinuxの起動に問題が出ることもあるので注意が必要ですが、最悪CUIでログインできれば元のバージョンに戻すとは可能です。)
カーネルのアップデートは以下コマンドでできます。

sudo apt-get install linux-generic-hwe-16.04

ds4drvを使わない

前述の通りds4drvは振動機能に対応していません。Linuxカーネル4.10以降ではds4drvを使わなくてもBluetoothでPCとペアリングが可能です。ペアリングの方法は、ds4drvを使う場合と同じくPSボタンとshareボタンを5秒くらい長押しし、ds4の青いランプがチカチカッと光るようになったら、PCのBluetoothの設定から新規デバイス追加を行います。

ペアリングが完了したら何もしなくてもds4drvで接続したのと同じ状態になっています。ROSを使う際もそのままrosrun joy joy_nodeを実行すればジョイスティックの値を見ることが可能です。

この状態でもう一度fftestによる振動テストを実行してみましょう。

$ fftest /dev/input/event19
Force feedback test program.
HOLD FIRMLY YOUR WHEEL OR JOYSTICK TO PREVENT DAMAGES

Device /dev/input/event19 opened
…
（中略）
…

Setting master gain to 75% ... OK
Uploading effect #0 (Periodic sinusoidal) ... OK (id 0)
Uploading effect #1 (Constant) ... Error: Invalid argument
Uploading effect #2 (Spring) ... Error: Invalid argument
Uploading effect #3 (Damper) ... Error: Invalid argument
Uploading effect #4 (Strong rumble, with heavy motor) ... OK (id 1)
Uploading effect #5 (Weak rumble, with light motor) ... OK (id 2)
Enter effect number, -1 to exit
0
Now Playing: Sine vibration
Enter effect number, -1 to exit

OKとなっている、#0、#4、#5が使えます。数字を入力するとコントローラが振動します。やったね！。

pythonプログラムから振動

kharlashkin氏のpythonプログラムです。いろんな強さ＆時間で振動させることができます。python2系で動作します。python3系で動かすときはself.ff_joy = open(file, "r+")の部分をself.ff_joy = open(file, "r+b")に変えるといいみたいです。他のプログラムやROSから使うときはpythonで振動させられるのは便利ですね。

import fcntl, struct, array, time

EVIOCRMFF = 0x40044581
EVIOCSFF = 0x40304580
LOG_CLASS_ON = False
TIME_DELTA = 250

class Vibrate:

    def __init__(self, file):
        self.ff_joy = open(file, "r+")

    def close(self):
        self.ff_joy.close()

    def new_effect(self, strong, weak, length):
        effect = struct.pack('HhHHHHHxHH', 0x50, -1, 0, 0, 0, length, 0, int(strong * 0xFFFF), int(weak * 0xFFFF))
        a = array.array('h', effect)
        fcntl.ioctl(self.ff_joy, EVIOCSFF, a, True)
        return a[1]
        id = a[1]
        return (ev_play, ev_stop)

    def play_efect(self, id):
        if type(id) == tuple or type(id) == list:
            ev_play = ''
            for i in id:
                ev_play = ev_play + struct.pack('LLHHi', 0, 0, 0x15, i, 1)
        else:
            ev_play = struct.pack('LLHHi', 0, 0, 0x15, id, 1)
        self.ff_joy.write(ev_play)
        self.ff_joy.flush()

    def stop_effect(self, id):
        if type(id) == tuple or type(id) == list:
            ev_stop = ''
            for i in id:
                ev_stop = ev_stop + struct.pack('LLHHi', 0, 0, 0x15, i, 0)
        else:
            ev_stop = struct.pack('LLHHi', 0, 0, 0x15, id, 0)
        self.ff_joy.write(ev_stop)
        self.ff_joy.flush()

    def forget_effect(self, id):
        if type(id) == tuple or type(id) == list:
            for i in id:
                fcntl.ioctl(self.ff_joy, EVIOCRMFF, i)
        else:
            fcntl.ioctl(self.ff_joy, EVIOCRMFF, id)

f = Vibrate("/dev/input/event14")
p = f.new_effect(1.0, 1.0, TIME_DELTA )
f.play_efect((p))
time.sleep(TIME_DELTA / 1000.0)
f.stop_effect((p))
f.forget_effect((p))

https://github.com/chrippa/ds4drv/issues/91から引用

今回は以上です。今後はこれを使って危険をユーザに知らせるアラートシステムを作っていきたいと思っています。振動させるROSノードが完成したらそちらも公開しようと思います。論文で大変なので時間があればですが…。