ROS- PS4 Joystick teleop

 

- 개발 환경 : Rasberry Pi 4B (Ubuntu 18.04 / ROS melodic)

- 준비물 : md200t, PS4 Controller, 블루투스 동글

- 참조 :  https://monotics.tistory.com/28

            https://github.com/engcang/PS4_Joystick_teleop_Mobile_Robots_ROS_Python

 

 

Step1. Bluetooth 연결

- Raspberry Pi USB 포트에 Bluetooth 동글을 연결합니다.

- PS4의 "SHARE" 버튼과 "PS4" 버튼을 동시에 눌러 페어링 모드로 진입합니다. (LED가 빠르게 반짝임)

- 터미널창에서 bluetooth 프롬포트를 실행하여 Wireless Controller를 연결합니다.

$ bluetoothctl

$ agent on
$ default-agent
$ power on
$ scan on
$ connect {Wireless Controller 값} 혹은 라즈베리파이의 bluetooth 창에서 연결

 

Step2. Joystick Set up and install

- Joy package를 다운로드합니다.

$ sudo apt-get install ros-melodic-joy

 

- access permission을 부여합니다.

$ roscore
$ rosrun joy joy_node
$ sudo chmod a+rw /dev/input/js0

 

- rostopic list와 echo를 이용하여 PS4 버튼을 누를 때 값이 변하는지 확인합니다. (잘 연결됐는지 확인)

$ rostopic list
$ rostopic echo /joy

rostopic echo /joy

Step3. Using code directly or as ROS node

- joy를 twist 값으로 바꿔줄 python 코드를 git clone 합니다.

$ git clone https://github.com/engcang/PS4_Joystick_teleop_Mobile_Robots_ROS_Python.git

 

- node와 py를 실행합니다.

$ roscore
$ roslaunch md md.launch
$ rosrun joy joy_node
$ python PS4_Joystick_teleop_Mobile_Robots_ROS_Python/Joyteleop_turtlebot3.py

 

 

Step4. Package 파일 작성 및 roslaunch 파일에 추가

- 먼저 본인에 맞게 py 파일 명 및 코드 수정 (morobot_joy_teleop.py)

# -*- coding: utf-8 -*-

import numpy as np
import rospy
import roslib
import subprocess
import time
from geometry_msgs.msg  import Twist
from sensor_msgs.msg import Joy
import sys
import signal

def signal_handler(signal, frame): # ctrl + c -> exit program
        print('You pressed Ctrl+C!')
        sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
''' class '''
class robot():
    def __init__(self):
        rospy.init_node('robot_controller', anonymous=True)
        self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.pose_subscriber2 = rospy.Subscriber('/joy',Joy,self.callback)
        self.rate = rospy.Rate(20)

    def callback(self, data):
        global inn
        inn=0
        self.joy = data.buttons
        self.joy2= data.axes
        if np.shape(self.joy)[0]>0:
            inn=1
            self.nemo=self.joy[3]
            self.semo=self.joy[2]
            self.one=self.joy[1]
            self.x=self.joy[0]
            self.R1=self.joy[5]
        if np.shape(self.joy2)[0]>0:
            inn=1
            self.linear=self.joy2[1]
            self.angular=self.joy2[0]
        if inn==1:
            if self.joy[0]==0 and self.joy[1]==0 and self.joy[2]==0 and self.joy[3]==0 and self.joy[5]==0 and self.joy2[0]==0 and self.joy2[1]==0:
                inn=0
            else:
                pass
    def moving(self,vel_msg):
        self.velocity_publisher.publish(vel_msg)

data=Joy()
vel_msg=Twist()

''' robot position '''
morobot = robot()
morobot.callback(data) #without this, getting error
global inn
inn=0

''' main '''
if __name__ == '__main__':
 while 1:
     if inn==1:
        if morobot.R1==1:
             vel_msg.linear.x=morobot.linear*0.22
             vel_msg.angular.z=morobot.angular*1.0
        morobot.moving(vel_msg)        
     morobot.rate.sleep()

 

 

- python 패키지 파일 만들기

# python 패키지 파일 생성

$ cd ~/catkin_ws/src

$ catkin_create_pkg morobot_joy_teleop rospy roslib sensor_msgs geometry_msgs

$ cd ~/catkin_ws && catkin_make

$ cd ~/catkin_ws/src/morobot_joy_teleop && mkdir scripts

# scripts 폴더에 morobot_joy_teleop.py파일 넣기

$ chmod +x morobot_joy_teleop.py

 

 

- launch 파일에 적용

# morobot_bringup_G2.launch 파일에 추가

<node pkg="morobot_joy_teleop" type="morobot_joy_teleop.py" name="morobot_joy_teleop" />