├── Joyteleop_turtlebot2.py
├── Joyteleop_turtlebot3.py
├── LICENSE
└── README.md


/Joyteleop_turtlebot2.py:
--------------------------------------------------------------------------------
 1 | # -*- coding: utf-8 -*-
 2 | """
 3 | Created on Sun Sep 30 20:02:45 2018
 4 | 
 5 | @author: mason
 6 | """
 7 | 
 8 | import numpy as np
 9 | import rospy
10 | import roslib
11 | import subprocess
12 | import time
13 | from geometry_msgs.msg  import Twist
14 | from sensor_msgs.msg import Joy
15 | import sys
16 | import signal
17 | 
18 | def signal_handler(signal, frame): # ctrl + c -> exit program
19 |         print('You pressed Ctrl+C!')
20 |         sys.exit(0)
21 | signal.signal(signal.SIGINT, signal_handler)
22 | ''' class '''
23 | class robot():
24 |     def __init__(self):
25 |         rospy.init_node('robot_controller', anonymous=True)
26 |         self.velocity_publisher = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=1)
27 |         self.pose_subscriber2 = rospy.Subscriber('/joy',Joy,self.callback)
28 |         self.rate = rospy.Rate(20)
29 | 
30 |     def callback(self, data):
31 |         global inn
32 |         inn=0
33 |         self.joy = data.buttons
34 |         self.joy2= data.axes
35 |         if np.shape(self.joy)[0]>0:
36 |             inn=1
37 |             self.nemo=self.joy[0]
38 |             self.semo=self.joy[3]
39 |             self.one=self.joy[2]
40 |             self.x=self.joy[1]
41 |         if np.shape(self.joy2)[0]>0:
42 |             inn=1
43 |             self.linear=self.joy2[1]
44 |             self.angular=self.joy2[0]
45 |         if inn==1:
46 |             if self.joy[0]==0 and self.joy[1]==0 and self.joy[2]==0 and self.joy[3]==0 and self.joy2[0]==0 and self.joy2[1]==0:
47 |                 inn=0
48 |             else:
49 |                 pass
50 |     def moving(self,vel_msg):
51 |         self.velocity_publisher.publish(vel_msg)
52 | 
53 | data=Joy()
54 | vel_msg=Twist()
55 | 
56 | ''' robot position '''
57 | turtle = robot()
58 | turtle.callback(data) #without this, getting error
59 | global inn
60 | inn=0
61 | 
62 | ''' main '''
63 | if __name__ == '__main__':
64 |  while 1:
65 |      if inn==1:
66 |         if turtle.nemo==1:
67 |              vel_msg.linear.x=turtle.linear*0.4
68 |              vel_msg.angular.z=turtle.angular*1.2
69 |         elif turtle.semo==1:
70 |              #subprocess.call('',shell=True)
71 |              p=subprocess.Popen('rostopic pub /mobile_base/commands/reset_odometry std_msgs/Empty "{}"',shell=True)
72 |              time.sleep(2)
73 |              p.terminate()
74 |         elif turtle.one==1:
75 |              vel_msg.linear.x=turtle.linear*0.7
76 |              vel_msg.angular.z=turtle.angular*2
77 |         elif turtle.x==1:
78 |              vel_msg.linear.x=0
79 |              vel_msg.angular.z=0
80 |         turtle.moving(vel_msg)        
81 |      else:
82 |          print('no data in')
83 |      turtle.rate.sleep()


--------------------------------------------------------------------------------
/Joyteleop_turtlebot3.py:
--------------------------------------------------------------------------------
 1 | # -*- coding: utf-8 -*-
 2 | """
 3 | Created on Sun Sep 30 20:02:45 2018
 4 | 
 5 | @author: mason
 6 | """
 7 | 
 8 | import numpy as np
 9 | import rospy
10 | import roslib
11 | import subprocess
12 | import time
13 | from geometry_msgs.msg  import Twist
14 | from sensor_msgs.msg import Joy
15 | import sys
16 | import signal
17 | 
18 | def signal_handler(signal, frame): # ctrl + c -> exit program
19 |         print('You pressed Ctrl+C!')
20 |         sys.exit(0)
21 | signal.signal(signal.SIGINT, signal_handler)
22 | ''' class '''
23 | class robot():
24 |     def __init__(self):
25 |         rospy.init_node('robot_controller', anonymous=True)
26 |         self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
27 |         self.pose_subscriber2 = rospy.Subscriber('/joy',Joy,self.callback)
28 |         self.rate = rospy.Rate(20)
29 | 
30 |     def callback(self, data):
31 |         global inn
32 |         inn=0
33 |         self.joy = data.buttons
34 |         self.joy2= data.axes
35 |         if np.shape(self.joy)[0]>0:
36 |             inn=1
37 |             self.nemo=self.joy[0]
38 |             self.semo=self.joy[3]
39 |             self.one=self.joy[2]
40 |             self.x=self.joy[1]
41 |         if np.shape(self.joy2)[0]>0:
42 |             inn=1
43 |             self.linear=self.joy2[1]
44 |             self.angular=self.joy2[0]
45 |         if inn==1:
46 |             if self.joy[0]==0 and self.joy[1]==0 and self.joy[2]==0 and self.joy[3]==0 and self.joy2[0]==0 and self.joy2[1]==0:
47 |                 inn=0
48 |             else:
49 |                 pass
50 |     def moving(self,vel_msg):
51 |         self.velocity_publisher.publish(vel_msg)
52 | 
53 | data=Joy()
54 | vel_msg=Twist()
55 | 
56 | ''' robot position '''
57 | turtle = robot()
58 | turtle.callback(data) #without this, getting error
59 | global inn
60 | inn=0
61 | 
62 | ''' main '''
63 | if __name__ == '__main__':
64 |  while 1:
65 |      if inn==1:
66 |         if turtle.nemo==1:
67 |              vel_msg.linear.x=turtle.linear*0.1
68 |              vel_msg.angular.z=turtle.angular*1.2
69 |         elif turtle.semo==1:
70 |              #subprocess.call('',shell=True)
71 |              p=subprocess.Popen('rostopic pub /reset std_msgs/Empty "{}"',shell=True)
72 |              time.sleep(2)
73 |              p.terminate()
74 |         elif turtle.one==1:
75 |              vel_msg.linear.x=turtle.linear*0.22
76 |              vel_msg.angular.z=turtle.angular*2
77 |         elif turtle.x==1:
78 |              vel_msg.linear.x=0
79 |              vel_msg.angular.z=0
80 |         turtle.moving(vel_msg)        
81 |      else:
82 |          print('no data in')
83 |      turtle.rate.sleep()


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2018, EungChang-Mason-Lee
 4 | All rights reserved.
 5 | 
 6 | Redistribution and use in source and binary forms, with or without
 7 | modification, are permitted provided that the following conditions are met:
 8 | 
 9 | 1. Redistributions of source code must retain the above copyright notice, this
10 |    list of conditions and the following disclaimer.
11 | 
12 | 2. Redistributions in binary form must reproduce the above copyright notice,
13 |    this list of conditions and the following disclaimer in the documentation
14 |    and/or other materials provided with the distribution.
15 | 
16 | 3. Neither the name of the copyright holder nor the names of its
17 |    contributors may be used to endorse or promote products derived from
18 |    this software without specific prior written permission.
19 | 
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # Joystick teleoperation for Mobile Robots using ROS, Python
  2 | + Simple Teleoperation using [PS4 from SONY](https://asia.playstation.com/ko-kr/accessories/dualshock4/)
  3 | <p align="left">
  4 | <img src="https://github.com/engcang/image-files/blob/master/joyteleop/ps4.png" width="300" hspace="50"/>
  5 | </p>
  6 | 
  7 | ## Index
  8 | + [Joystick Set up](#-joystick-set-up-and-install)
  9 | + [Code explanation](#-code-explanation)
 10 | + [Using code as ROS node](#-using-code-directly-or-as-ros-node)
 11 | + [Robot Teleoperation Result](#-result-data-for-turtlebot2-and-turtlebot3)
 12 | 
 13 | <br>
 14 | 
 15 | ## ● Joystick Set up and install
 16 | + First install [**Joy** package](http://wiki.ros.org/joy) by
 17 |   ~~~shell
 18 |   $ sudo apt-get install ros-<your distro version>-joy
 19 |   ~~~
 20 | <br>
 21 | 
 22 | + Do bluetooth pairing
 23 | <p align="left">
 24 | <img src="https://github.com/engcang/image-files/blob/master/joyteleop/pairing.JPG" width="400" hspace="80"/>
 25 | </p>
 26 | <br>
 27 | 
 28 | + After that, give permission to access bluetooth controller
 29 |   ~~~shell
 30 |   $ roscore
 31 |   $ rosrun joy joy_node
 32 |   $ sudo chmod a+rw /dev/input/js0
 33 |   ~~~
 34 |   </br>
 35 | 
 36 | + and check the data comes in using **rostopic echo**, while moving stick and pushing buttons of PS4
 37 |   ~~~shell
 38 |   $ rostopic list
 39 |   $ rostopic echo /joy
 40 |   ~~~
 41 | <p align="center">
 42 | <img src="https://github.com/engcang/image-files/blob/master/joyteleop/joy_echo.png" width="500" hspace="0"/>
 43 | </p>
 44 | <br>
 45 | 
 46 | + data.buttons[0] is _rectangle_ button, [1] is _x_ button, [2] is _circle_ button and [3] is _triangle_ button each, the picture shows when I pushed _circle_ button <br>
 47 | + data.axes[0] and data.axes[1] are Left stick's data corresponding to angular and linear _velocities_ each
 48 | 
 49 | <br><br><br>
 50 | 
 51 | ## ● Code explanation
 52 | + Import libraries and init class
 53 |   ~~~python
 54 |   import numpy as np
 55 |   import rospy
 56 |   import roslib
 57 |   import subprocess
 58 |   import time
 59 |   from geometry_msgs.msg  import Twist
 60 |   from sensor_msgs.msg import Joy
 61 |   import sys
 62 |   import signal
 63 | 
 64 |   def signal_handler(signal, frame): # ctrl + c -> exit program
 65 |           print('You pressed Ctrl+C!')
 66 |           sys.exit(0)
 67 |   signal.signal(signal.SIGINT, signal_handler)
 68 |   ''' class '''
 69 |   class robot():
 70 |       def __init__(self):
 71 |           rospy.init_node('robot_controller', anonymous=True)
 72 |           self.velocity_publisher = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=1)
 73 |           self.pose_subscriber2 = rospy.Subscriber('/joy',Joy,self.callback)
 74 |           self.rate = rospy.Rate(20)
 75 |   ~~~
 76 |   1.for turtlebot2, used **/mobile_base/commands/velocity** as velocity publishing topic, for turtlebot 3, should use **/cmd_vel** <br>
 77 |   Both codes were uploaded on this repository. <br>
 78 |   2.To get joystick's data, make subscriber under **'/joy'** topic
 79 | 
 80 | <br>
 81 | 
 82 | + Class methods
 83 |   ~~~python
 84 |       def callback(self, data):
 85 |         global inn
 86 |         inn=0
 87 |         self.joy = data.buttons
 88 |         self.joy2= data.axes
 89 |         if np.shape(self.joy)[0]>0:
 90 |             inn=1
 91 |             self.nemo=self.joy[0]
 92 |             self.semo=self.joy[3]
 93 |             self.one=self.joy[2]
 94 |             self.x=self.joy[1]
 95 |         if np.shape(self.joy2)[0]>0:
 96 |             inn=1
 97 |             self.linear=self.joy2[1]
 98 |             self.angular=self.joy2[0]
 99 |         if inn==1:
100 |             if self.joy[0]==0 and self.joy[1]==0 and self.joy[2]==0 and self.joy[3]==0 and self.joy2[0]==0 and self.joy2[1]==0:
101 |                 inn=0
102 |             else:
103 |                 pass
104 |     def moving(self,vel_msg):
105 |         self.velocity_publisher.publish(vel_msg)
106 | 
107 |   ~~~
108 |   3._**Callback**_ function is automatically implemented whenever data comes in under **joy** topic <br>
109 |   4.When joy data is not empty, **globall inn** is replaced with **'1'** <br>
110 | 
111 | <br>
112 | 
113 | + main code
114 |   ~~~python
115 |   ''' main '''
116 |   if __name__ == '__main__':
117 |    while 1:
118 |        if inn==1:
119 |           if turtle.nemo==1:
120 |                vel_msg.linear.x=turtle.linear*0.4
121 |                vel_msg.angular.z=turtle.angular*1.2
122 |           elif turtle.semo==1:
123 |                #subprocess.call('',shell=True)
124 |                p=subprocess.Popen('rostopic pub /mobile_base/commands/reset_odometry std_msgs/Empty "{}"',shell=True)
125 |                time.sleep(2)
126 |                p.terminate()
127 |           elif turtle.one==1:
128 |                vel_msg.linear.x=turtle.linear*0.7
129 |                vel_msg.angular.z=turtle.angular*2
130 |           elif turtle.x==1:
131 |                vel_msg.linear.x=0
132 |                vel_msg.angular.z=0
133 |           turtle.moving(vel_msg)        
134 |        else:
135 |            print('no data in')
136 |        turtle.rate.sleep()
137 |   ~~~
138 |   5.when **global inn** is '1', do any tasks using joy buttons data and joy axes data <br>
139 |   6.when **rectangle** button is pushed, move slowly and **circle** button is pushed, move fast <br>
140 |   **X** button is pushed, stop and **triangle** button is pushed, reset _**Odometry**_ <br>
141 | 
142 | <br>
143 | 
144 | ## ● Using code directly or as ROS node
145 | + git clone the codes first
146 |   ~~~shell
147 |   $ git clone https://github.com/engcang/PS4_Joystick_teleop_Mobile_Robots_ROS_Python.git
148 |   ~~~
149 | <br>
150 | 
151 | + Run the code directly with ROS Master
152 |   ~~~shell
153 |   $ roslaunch turtlebot_bringup minimal.launch
154 |   $ rosrun joy joy_node
155 |   $ python PS4_Joystick_teleop_Mobile_Robots_ROS_Python/Joyteleop_turtlebot2.py
156 |   ~~~
157 |   1.**Have to run robot_bringup first!** <br>
158 |   2.**Have to run joy_node** to get joystick data <br>
159 |   3.run python code
160 |   
161 | <br>
162 | 
163 | + Run the code after make it as Node
164 |   ~~~shell
165 |   $ cd ~/catkin_ws/src
166 |   $ catkin_create_pkg <name> rospy roslib sensor_msgs geometry_msgs
167 |   $ cd ~/catkin_ws && catkin_make
168 |   $ cd ~/catkin_ws/src/<name> && mkdir scripts
169 |   $ mv ~/PS4_Joystick_teleop_Mobile_Robots_ROS_Python/Joyteleop_turtlebot2.py ~/catkin_ws/src/<name>/scripts
170 |   $ chmod +x Joyteleop_turtlebot2.py
171 |   $ roscore
172 |   $ rosrun <name> Joyteleop_turtlebot2.py
173 |   ~~~
174 | <br>
175 | 
176 | + Run the code by ROS launch
177 |   ~~~xml
178 |   <node pkg="<name>" type="Joyteleop_turtlebot2.py" name="Joyteleop_turtlebot2" />
179 |   ~~~
180 |   Simply add this line into launch file you want to launch together
181 | 
182 | <br>
183 | 
184 | ## ● Result for turtlebot2 and turtlebot3
185 | <p align="center">
186 | <img src="https://github.com/engcang/image-files/blob/master/joyteleop/Joy_TB3.gif"/>
187 | </p>
188 | <p align="center">
189 | <img src="https://github.com/engcang/image-files/blob/master/joyteleop/Joy_TB2.gif"/>
190 | </p>
191 | 


--------------------------------------------------------------------------------