ROSの勉強　第42弾：ps4のコントローラでカメ操作

# #プログラミング ROS< ps4のコントローラでカメ操作 >

## はじめに
ROS(Robot Operating System)をさらに扱えるようになることが目的である．その第42弾として，「ps4のコントローラでカメ操作」を扱う．

## 環境
##### コンピュータ
|||
|:-:|:--|
|デバイス|raspberry pi 4 Model B|
|メモリ|3.7 GiB|
|Graphics|V3D 4.2|
|OS|Ubuntu|

#####Ubuntu
|||
|:-:|:--|
|リリース|20.04.2 LTS (Focal Fossa) 64ビット|
|カーネル|Linux 5.4.0-1035-raspi aarch64|
|MATE|1.24.0|

##### ROS
|||
|:-:|:--|
|Distribution|noetic|

##### コントローラ
|||
|:-:|:--|
|メーカー|SONY|
|シリーズ|DUALSHOCK4 Wireless Controller|

## ジョイパッド
ジョイパッドやジョイスティックはロボットを遠隔で操作するためによく使われる．ここでは，PS4のコントローラを使って，シミュレーション状のカメを動かす．

## 環境構築
### 必要なパッケージをインストール
#### ds4drv
```
sudo pip install ds4drv
```
#### joy
```
sudo apt install ros-noetic-joy
```
これで準備できた．今回は使っていないが，joyの代わりに次のようなパッケージも存在するようだ．
#### pyPS4Controller (option)
```
sudo pip install pyPS4Controller
```
参考

https://github.com/ArturSpirin/pyPS4Controller
## 接続手順
#### ①PCのBluetooth機能をオンであることを確認する
#### ②ds4drvを起動
```
sudo ds4drv
```
#### ③PS4コントローラを接続
PSボタンとSHAREボタンを同時に押す．すると，PS4コントローラのライトが白く点滅する．あとは，接続するのを待つだけ．

これで，PS4のコントローラは接続できたが，まだ入力情報は取得できない．そこで，joyが登場する．
### PS4の入力情報を配信する手順
#### ①roscoreでマスタを立ち上げる
```
roscore
```
#### ②joyノードを起動
```
rosrun joy joy_node
```
これで，/joyにトピックを配信する．
``rostopic echo /joy``により，コントローラからの入力を確認することができる．

### /joyの配信トピックとps4ボタンの対応
![ps4buttons.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/b3c69aae-fb44-65ee-c9b9-ce57e9a84fb8.png)
![ps4axes.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/a32797c2-a044-03b5-247e-cba8c4116e93.png)
なお，axesについては，13以降ボタンを認識しており，6～12についてはよくわからない．なので，すべてを説明はできていない．しかし，必要になってくるであろう部分は網羅できていると考える．また，タッチパッドについては認識していなかった．タッチパッドも対応させる方法はあったはずだが，調べていない．

## 実装
ここでは，ps4のコントローラを用いて，turtlesimを動かすので，必要な実行ファイルは次のとおりである．

### 概要
* ds4drv
* joy_node
* turtlesim
* joy_to_twist

turtlesimは/turtlesim1/cmd_velというトピックを購読しており，そのトピックの配信情報に応じて動く．一方で，joy_nodeは/joyというトピックを配信している．本当は，コントローラの入力すなわち/joyの配信情報を/turtlesim1/cmd_velに伝えたい．その仲介を担うのに必要なのが，joy_to_twistである．これは，既存のプログラムではなく，単に「/joyを購読してcmd_velを配信する」という自作プログラムである．これは，よくコントローラを用いるときに作られるものである．過去にkey_to_twistを学習した．

https://qiita.com/Yuya-Shimizu/items/bf58f6fa0925b5460022

これを参考に作っていくことにする．

### joy_to_twist
今回は，左のジョイスティックだけを使って，カメを動かしていくので，非常にシンプルなjoy_to_twistプログラムとなった．

```python:joy_to_twist.py
#! /usr/bin/env python3

import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist

def control(msg, twist_pub):#joy to twist
    L_horizontal = msg.axes[0]  #左ジョイスティック（左右）
    L_vertical = msg.axes[1]    #左ジョイスティック（上下）
    circle = msg.buttons[13]
    velocity = [L_horizontal*(1+circle), L_vertical*(1+circle)]

    t = Twist() #Twistのインスタンスを生成
 
    t.angular.z, t.linear.x = velocity #twistにjoyから取得したデータを当てはめる

    twist_pub.publish(t)    #twistを配信

if __name__ == '__main__':
    #ノードの初期化
    rospy.init_node('joy_to_twist')
    #配信準備
    twist_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=1)
    #購読
    rospy.Subscriber('/joy', Joy, control, twist_pub)

    rospy.spin()
```

### launchファイル
実行するプログラムはそろったので，launchファイルにまとめておく．ただし，ds4drvは含めない．これは，はじめにps4のコントローラに接続するものであるから，コマンドラインで実行する．

```xml:joy_lesson.launch
<launch>
    <include file="$(find joy)/launch/ps4joy.launch"/>
    <node name="turtle" pkg="turtlesim" type="turtlesim_node" output="screen"/>
    <node name="joy_to_twist" pkg="joy_lesson" type="joy_to_twist.py" output="screen"/>
</launch>
```

実行してみる

```
roslaunch joy_lesson joy_lesson.launch
```

実際につながったか確認する．

```
rostopic info /turtlesim1/cmd_vel
```
![cmd_vel.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/10c2f757-dec9-8910-3df2-47724ed5483d.png)

確かに，cmd_velがjoyの入力をjoy_to_twistを介して受け取れていることが確認できる．

## 実行
```:ps4コントローラ接続
sudo ds4drv
```

```:USBポートにアクセス権限を与える
sudo chmod 666 /dev/ttyUSB0
```

```:実行
roslaunch joy_lesson joy_lesson.launch
```

これらすべての実行様子を動画像で下に示す．
<iframe width="560" height="315" src="https://www.youtube.com/embed/SlSOfPbIxVI" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

上で示した``joy_to_twist.py``にあるように，基本的に左ジョイスティックで操作し，〇を押しながら操作するとカメが2倍の速さで進むようにプログラムしている．うまくカメが操作できていることが分かる．

## 感想
ようやく，ROS上でのps4のコントローラの扱い方を理解した．過去に一度挑戦したことはあったが，ROS自体があまり理解できていなかったこともあり，挫折したが，今回でしっかりと理解できた．今回は，シミュレーションのカメを動かすだけであったが，近々，実際のロボットを動かすつもりである．

## 参考文献
実用ロボット開発のためのROSプログラミング
　　　　　　　　西田 健，森田 賢，岡田 浩之，原 祥尭，
　　　　　　　　山崎 公俊，田向 権，垣内 洋平，大川 一也，
　　　　　　　　斎藤 功，田中 良道，有田 裕太，石田 祐太郎 共著
　　　　　　　　　　　　　　　　　　　　 森北出版株式会社　発行