Help us understand the problem. What is going on with this article?

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

はじめに

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

環境

  • ubuntu 16.04
  • ROS kinetic
  • Python 2.7
  • pygame

できること(動画)

以下のツイートがスレッドになってしまってます。すみません。。。
ジョイコンのスティック操作からTurtlebot3をgazebo上で動かしています。(小さくてすみません)

teleop_twist_keyboardのようにボタンで速度を変えられるようになっています。

準備

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

$ pip install pygame

BluetoothでPCとジョイコンを接続してください。下図のボタンでBlutooth接続モードになります。
image.png

コード

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

#! /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 == ***の中ですね。ジョイスティックとボタンを使っています。

self.joystick0.get_hat(0)

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

方向 x y
-1 0
1 0
0 1
0 -1

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

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

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

おわりに

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

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

参考

koichi_baseball
大学院生 ロボットの研究とベンチャーのデータ会社でインターン中 Robot、ROS、Open-RTM勉強中 プログラミング言語はPythonを主に使ってます
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした