Nintendo Switchのジョイコン(L)をROSのコントローラとして使う

# はじめに
SwitchのジョイコンからROSの`/cmd_vel`トピックに送ることができるノードを作りました。
ジョイスティックを前に倒した時に前進、後ろに倒した時に後退、右に倒した時に右旋回、左に倒した時に左旋回するようになっています。

# 環境
 - ubuntu 16.04
 - ROS kinetic
 - Python 2.7
 - pygame

# できること(動画)
以下のツイートがスレッドになってしまってます。すみません。。。
ジョイコンのスティック操作からTurtlebot3をgazebo上で動かしています。(小さくてすみません)
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">turtlebot3(とても小さい) <a href="https://t.co/Ju2Vk2zaQF">pic.twitter.com/Ju2Vk2zaQF</a></p>&mdash; いち (@_first_blog) <a href="https://twitter.com/_first_blog/status/1258723914043125760?ref_src=twsrc%5Etfw">May 8, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

[teleop_twist_keyboard](http://wiki.ros.org/teleop_twist_keyboard)のようにボタンで速度を変えられるようになっています。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ボタンで速度を変える(teleop_twist_keyboardのイメージ) <a href="https://t.co/c54soXScCB">pic.twitter.com/c54soXScCB</a></p>&mdash; いち (@_first_blog) <a href="https://twitter.com/_first_blog/status/1258730325305200643?ref_src=twsrc%5Etfw">May 8, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# 準備
pygameが入っていない方は以下コマンドでインストールしてください。

```bash
$ pip install pygame
```

BluetoothでPCとジョイコンを接続してください。下図のボタンでBlutooth接続モードになります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/426354/21629220-d55b-9108-5196-9e23f6d03328.png)


# コード
これをコピペすればすぐ使えます。

```Python
#! /usr/bin/env python
# -*- coding: utf-8 -*-
 
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

import pygame
from pygame.locals import *
import time

class joyconController:
    def __init__(self):
        rospy.init_node('joycon_node', anonymous=True)
        self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1000)
        rospy.Timer(rospy.Duration(0.1), self.timerCallback)

        # ジョイコン操作用
        pygame.joystick.init()
        self.joystick0 = pygame.joystick.Joystick(0)
        self.joystick0.init()
        self.joystickx = 0
        self.joysticky = 0
        pygame.init()

        # ボタンのカウントと速度の係数
        self.button_cnt = 0
        self.coff_linearx = 1
        self.coff_angularz = 1

        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0
        self.twist_pub.publish(twist)

        print 
        print '▲ : UP Linear X,  ▼ : DOWN Linear X'
        print '◀ : UP Angular Z, ▶ : DOWN Angular Z'
        print
 
    def timerCallback(self, event):
        twist = Twist()
        eventlist = pygame.event.get()

        # イベント処理
        for e in eventlist:
            if e.type == QUIT:
                return
            # スティックの処理
            if e.type == pygame.locals.JOYHATMOTION:
                self.button_cnt += 1
                self.joystickx, self.joysticky = self.joystick0.get_hat(0)
                print 'linear x:' + str(-self.joystickx * self.coff_linearx) + ', angular z:' + str(-self.joysticky * self.coff_angularz)
            # ボタンの処理
            elif e.type == pygame.locals.JOYBUTTONDOWN:
                self.button_cnt += 1
                if e.button==2:
                    self.coff_linearx += 0.1
                elif e.button==1:
                    self.coff_linearx -= 0.1
                    if self.coff_linearx <=0.1: self.coff_linearx = 0.1
                elif e.button==0:
                    self.coff_angularz += 0.1
                elif e.button==3:
                    self.coff_angularz -= 0.1
                    if self.coff_angularz <= 0.1: self.coff_angularz = 0.1
                print 'linear x:' + str(self.coff_linearx) + ', angular z:' + str(self.coff_angularz)

        twist.linear.x  = -self.joystickx * self.coff_linearx
        twist.angular.z = -self.joysticky * self.coff_angularz

        self.twist_pub.publish(twist)

        if self.button_cnt>=10:
            print 
            print '▲ : UP Linear X,  ▼ : DOWN Linear X'
            print '◀ : UP Angular Z, ▶ : DOWN Angular Z'
            print
            self.button_cnt = 0

if __name__ == '__main__':
    try:
        jc = joyconController()
        rospy.spin()
    except pygame.error:
        print 'joystickが見つかりませんでした。'
    except rospy.ROSInterruptException: pass
```

# 簡単に解説
ジョイコンの入力をとってきている部分は`timerCallback`の中の`e.type == ***`の中ですね。ジョイスティックとボタンを使っています。

```Python
self.joystick0.get_hat(0)
```

ジョイスティックの返り値は縦持ちしたとき以下のようになっています。

| 方向 | x | y |
|:-:|:-:|:-:|
| 前 | -1 | 0 |
| 後 | 1 | 0 |
| 右 | 0 | 1 |
| 左 | 0 | -1 |

>前に倒した時に前進、後ろに倒した時に後退、右に倒した時に右旋回、左に倒した時に左旋回するようになっています。

このようにするため、返り値に-1をかけています。

また、ジョイスティックの入力の値は0か1であるため、それぞれのボタン▲▼▶◀で速度も操作できるようにしました。

# おわりに
ジョイコンでの操作、やってみたかったのでやってみました！思ったより簡単にできて解説もチープですがなにかあれば追記します。
修正点などもろもろあればコメントください。

自粛期間開けたら研究室で実際のロボット動かしたい～

# 参考
 - [【Python】Pygame で Nintendo Switch の Joy-Con(L) の操作を検出](https://qiita.com/Kazuhito/items/403d84577cdff1a3ba91)
 - [Nintendo Switch の Joy-Con で家電を操作する](https://qiita.com/taznica/items/a4f5123f84641e4cac40)