Skip to content

Commit 3451af8

Browse files
nathantsoinathantsoi
committed
docs for sim, fix teleop
1 parent f663cf6 commit 3451af8

4 files changed

Lines changed: 292 additions & 227 deletions

File tree

‎scripts/joystick_teleop.py‎

Lines changed: 163 additions & 162 deletions
Original file line numberDiff line numberDiff line change
@@ -1,175 +1,176 @@
1-
#!/usr/bin/env python
2-
import rospy
1+
#!/usr/bin/env python3
2+
import rclpy
3+
from rclpy.node import Node
4+
5+
# disable ALSA audio driver for pygame
6+
import os
7+
os.environ['SDL_AUDIODRIVER'] = 'dsp'
8+
39
import pygame
410
import time
511

6-
from ackermann_msgs.msg import AckermannDriveStamped
12+
from amrl_msgs.msg import AckermannCurvatureDriveMsg
713
from sensor_msgs.msg import Joy
814

9-
import sys, select, termios, tty
15+
import sys
1016

1117
# See the pygame docs for more joystick capabilities:
1218
# https://www.pygame.org/docs/ref/joystick.html
1319

1420
INACTIVITY_RECONNECT_TIME = 5
1521
RECONNECT_TIMEOUT = 2
16-
steer_joystick = 0.0
17-
drive_joystick = 0.0
18-
is_enabled = False
19-
turbo_mode = False
20-
joystick_connected = False
21-
last_active_time = 0
22-
last_reconnect_try_time = 0
23-
24-
25-
def checkConnectivity():
26-
global joystick_connected
27-
global joystick
28-
global last_active_time
29-
global last_reconnect_try_time
30-
31-
now = time.time()
32-
if ((now - last_active_time > INACTIVITY_RECONNECT_TIME) and
33-
(now - last_reconnect_try_time > RECONNECT_TIMEOUT)):
34-
last_reconnect_try_time = now
35-
print("Checking joystick connectivity.")
36-
pygame.joystick.quit()
37-
pygame.joystick.init()
38-
joystick_connected = False
39-
40-
joystick_count = pygame.joystick.get_count()
41-
42-
if (joystick_count < 1):
43-
joystick_connected = False
44-
print("****** Joystick disconnected ******\n")
45-
46-
if ((not joystick_connected) and (joystick_count > 0)):
47-
joystick.quit()
48-
joystick = pygame.joystick.Joystick(0)
49-
joystick.init()
50-
joystick_connected = True
51-
print("****** Joystick Reconnected ******\n")
52-
53-
def readJoystick():
54-
global joystick
55-
global steer_joystick
56-
global drive_joystick
57-
global is_enabled
58-
global turbo_mode
59-
global pub_joymsg
60-
global last_active_time
61-
pygame.event.pump()
62-
steer_joystick = -joystick.get_axis(0)
63-
drive_joystick = -joystick.get_axis(4) # 4 for xbox
64-
is_enabled = joystick.get_button(4) == 1
65-
turbo_mode = joystick.get_axis(2) >= 0.9 # 5 for xbox
66-
67-
# Makes the joystick message
68-
joy_msg = Joy();
69-
joy_msg.header.stamp = rospy.Time.now();
70-
joy_msg.header.frame_id = "base_link";
71-
72-
for i in range(6): #7
73-
joy_msg.axes += [joystick.get_axis(i)]
74-
#if (joystick.get_axis(i)):
75-
# last_active_time = time.time()
76-
for i in range(10): #10
77-
joy_msg.buttons += [joystick.get_button(i)]
78-
if (joystick.get_button(i)):
79-
last_active_time = time.time()
80-
pub_joymsg.publish(joy_msg)
81-
82-
def initJoystick():
83-
global joystick
84-
global joystick_connected
85-
pygame.init()
86-
87-
# Initialize the joysticks
88-
pygame.joystick.init()
89-
# Get count of joysticks
90-
joystick_count = pygame.joystick.get_count()
91-
92-
if joystick_count < 1:
93-
print('No joystick found!')
94-
sys.exit(0)
95-
96-
joystick = pygame.joystick.Joystick(0)
97-
joystick.init()
98-
joystick_connected = True
99-
100-
# Get the name from the OS for the controller/joystick
101-
name = joystick.get_name()
102-
print("Joystick name: {}".format(name) )
103-
104-
if (joystick.get_numbuttons() < 5):
105-
print("Error: expected buttion[4] to be valid!")
106-
sys.exit(1)
107-
108-
if (joystick.get_numaxes() < 4):
109-
print("Error: expected axis[0] and axis[3] to be valid!")
110-
sys.exit(1)
22+
23+
class JoystickTeleop(Node):
24+
def __init__(self):
25+
super().__init__('joystick_teleop')
26+
27+
# Initialize class variables
28+
self.steer_joystick = 0.0
29+
self.drive_joystick = 0.0
30+
self.is_enabled = False
31+
self.turbo_mode = False
32+
self.joystick_connected = False
33+
self.last_active_time = 0
34+
self.last_reconnect_try_time = 0
35+
self.joystick = None
36+
37+
# Create publishers
38+
self.pub = self.create_publisher(AckermannCurvatureDriveMsg, 'ackermann_curvature_drive', 5)
39+
self.pub_joymsg = self.create_publisher(Joy, '/bluetooth_teleop/joy', 5)
40+
41+
# Initialize joystick
42+
self.initJoystick()
43+
44+
# Create timer for main loop (20Hz)
45+
self.timer = self.create_timer(0.05, self.timer_callback)
46+
47+
self.last_active_time = time.time()
48+
self.last_reconnect_try_time = time.time()
49+
self.speed = 1.0
50+
self.turn = 0.25
51+
52+
def checkConnectivity(self):
53+
now = time.time()
54+
if ((now - self.last_active_time > INACTIVITY_RECONNECT_TIME) and
55+
(now - self.last_reconnect_try_time > RECONNECT_TIMEOUT)):
56+
self.last_reconnect_try_time = now
57+
print("Checking joystick connectivity.")
58+
pygame.joystick.quit()
59+
pygame.joystick.init()
60+
self.joystick_connected = False
61+
62+
joystick_count = pygame.joystick.get_count()
63+
64+
if (joystick_count < 1):
65+
self.joystick_connected = False
66+
print("****** Joystick disconnected ******\n")
67+
68+
if ((not self.joystick_connected) and (joystick_count > 0)):
69+
self.joystick.quit()
70+
self.joystick = pygame.joystick.Joystick(0)
71+
self.joystick.init()
72+
self.joystick_connected = True
73+
print("****** Joystick Reconnected ******\n")
74+
75+
def readJoystick(self):
76+
pygame.event.pump()
77+
self.steer_joystick = -self.joystick.get_axis(0)
78+
self.drive_joystick = -self.joystick.get_axis(4) # 4 for xbox
79+
self.is_enabled = self.joystick.get_button(4) == 1
80+
self.turbo_mode = self.joystick.get_axis(2) >= 0.9 # 5 for xbox
81+
82+
# Makes the joystick message
83+
joy_msg = Joy()
84+
joy_msg.header.stamp = self.get_clock().now().to_msg()
85+
joy_msg.header.frame_id = "base_link"
86+
87+
for i in range(6): #7
88+
joy_msg.axes.append(self.joystick.get_axis(i))
89+
#if (self.joystick.get_axis(i)):
90+
# self.last_active_time = time.time()
91+
for i in range(10): #10
92+
joy_msg.buttons.append(self.joystick.get_button(i))
93+
if (self.joystick.get_button(i)):
94+
self.last_active_time = time.time()
95+
self.pub_joymsg.publish(joy_msg)
96+
97+
def initJoystick(self):
98+
pygame.init()
99+
100+
# Initialize the joysticks
101+
pygame.joystick.init()
102+
# Get count of joysticks
103+
joystick_count = pygame.joystick.get_count()
104+
105+
if joystick_count < 1:
106+
print('No joystick found!')
107+
print('Please connect a joystick and try again.')
108+
print('If your joystick is connected, ensure that the udev rules are set up correctly.')
109+
print('You can run the script "src/ut_automata/scripts/udev.sh" to set up the appropriate rule.')
110+
sys.exit(0)
111+
112+
self.joystick = pygame.joystick.Joystick(0)
113+
self.joystick.init()
114+
self.joystick_connected = True
115+
116+
# Get the name from the OS for the controller/joystick
117+
name = self.joystick.get_name()
118+
print("Joystick name: {}".format(name) )
119+
120+
if (self.joystick.get_numbuttons() < 5):
121+
print("Error: expected buttion[4] to be valid!")
122+
sys.exit(1)
123+
124+
if (self.joystick.get_numaxes() < 4):
125+
print("Error: expected axis[0] and axis[3] to be valid!")
126+
sys.exit(1)
127+
128+
def timer_callback(self):
129+
self.checkConnectivity()
130+
131+
if self.joystick_connected:
132+
self.readJoystick()
133+
if self.turbo_mode:
134+
speed = 2.0
135+
else:
136+
speed = 1.0
137+
138+
msg = AckermannCurvatureDriveMsg()
139+
msg.header.stamp = self.get_clock().now().to_msg()
140+
msg.header.frame_id = "base_link"
141+
142+
print("Drive: {:.2f}% Steer: {:.2f}% Enabled: {}".format(
143+
self.drive_joystick, self.steer_joystick, self.is_enabled))
144+
145+
msg.velocity = self.drive_joystick * speed
146+
msg.curvature = self.steer_joystick * self.turn
147+
148+
if self.is_enabled:
149+
self.pub.publish(msg)
150+
151+
def main(args=None):
152+
rclpy.init(args=args)
153+
154+
joystick_teleop = JoystickTeleop()
155+
156+
try:
157+
rclpy.spin(joystick_teleop)
158+
except KeyboardInterrupt:
159+
pass
160+
finally:
161+
# Send stop message
162+
msg = AckermannCurvatureDriveMsg()
163+
msg.header.stamp = joystick_teleop.get_clock().now().to_msg()
164+
msg.header.frame_id = "base_link"
165+
msg.velocity = 0.0
166+
msg.curvature = 0.0
167+
joystick_teleop.pub.publish(msg)
168+
169+
# Close joystick
170+
pygame.quit()
171+
172+
joystick_teleop.destroy_node()
173+
rclpy.shutdown()
111174

112175
if __name__=="__main__":
113-
global steer_joystick
114-
global drive_joystick
115-
global is_enabled
116-
global turbo_mode
117-
global pub_joymsg
118-
pub = rospy.Publisher('commands/ackermann',
119-
AckermannDriveStamped,
120-
queue_size=5)
121-
pub_joymsg = rospy.Publisher('/bluetooth_teleop/joy',
122-
Joy,
123-
queue_size=5)
124-
rospy.init_node('joystick_teleop')
125-
rate = rospy.Rate(20) # 20hz
126-
initJoystick()
127-
128-
last_active_time = time.time()
129-
last_reconnect_try_time = time.time()
130-
speed = 1.0 # 1.0
131-
132-
turn = 0.25
133-
while not rospy.is_shutdown():
134-
checkConnectivity()
135-
136-
if joystick_connected:
137-
readJoystick()
138-
if turbo_mode:
139-
speed = 2.0
140-
else:
141-
speed = 1.0
142-
143-
msg = AckermannDriveStamped();
144-
msg.header.stamp = rospy.Time.now();
145-
msg.header.frame_id = "base_link";
146-
147-
print("Drive: {:.2f}% Steer: {:.2f}% Enabled: {}".format(
148-
drive_joystick, steer_joystick, is_enabled))
149-
150-
msg.drive.speed = drive_joystick * speed;
151-
msg.drive.acceleration = 1;
152-
msg.drive.jerk = 1;
153-
msg.drive.steering_angle = steer_joystick * turn
154-
msg.drive.steering_angle_velocity = 1
155-
156-
if is_enabled:
157-
pub.publish(msg)
158-
159-
rate.sleep()
160-
161-
msg = AckermannDriveStamped();
162-
msg.header.stamp = rospy.Time.now();
163-
msg.header.frame_id = "base_link";
164-
165-
msg.drive.speed = 0;
166-
msg.drive.acceleration = 1;
167-
msg.drive.jerk = 1;
168-
msg.drive.steering_angle = 0
169-
msg.drive.steering_angle_velocity = 1
170-
pub.publish(msg)
171-
172-
# Close the window and quit.
173-
# If you forget this line, the program will 'hang'
174-
# on exit if running from IDLE.
175-
pygame.quit ()
176+
main()

0 commit comments

Comments
 (0)