はじめに
SwitchのジョイコンからROSの/cmd_velトピックに送ることができるノードを作りました。
ジョイスティックを前に倒した時に前進、後ろに倒した時に後退、右に倒した時に右旋回、左に倒した時に左旋回するようになっています。
環境
- ubuntu 16.04
- ROS kinetic
- Python 2.7
- pygame
できること(動画)
以下のツイートがスレッドになってしまってます。すみません。。。
ジョイコンのスティック操作からTurtlebot3をgazebo上で動かしています。(小さくてすみません)
turtlebot3(とても小さい) pic.twitter.com/Ju2Vk2zaQF
— いち (@_first_blog) May 8, 2020
teleop_twist_keyboardのようにボタンで速度を変えられるようになっています。
ボタンで速度を変える(teleop_twist_keyboardのイメージ) pic.twitter.com/c54soXScCB
— いち (@_first_blog) May 8, 2020
準備
pygameが入っていない方は以下コマンドでインストールしてください。
$ pip install pygame
BluetoothでPCとジョイコンを接続してください。下図のボタンでBlutooth接続モードになります。
コード
これをコピペすればすぐ使えます。
#! /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であるため、それぞれのボタン▲▼▶◀で速度も操作できるようにしました。
おわりに
ジョイコンでの操作、やってみたかったのでやってみました！思ったより簡単にできて解説もチープですがなにかあれば追記します。
修正点などもろもろあればコメントください。
自粛期間開けたら研究室で実際のロボット動かしたい～