forked from HKUST-Aerial-Robotics/Teach-Repeat-Replan
-
Notifications
You must be signed in to change notification settings - Fork 0
/
key2joy.py
126 lines (114 loc) · 4.02 KB
/
key2joy.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
import pygame
from pygame.locals import *
import time
import sys
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
def main():
# initialize pygame to get keyboard event
pygame.init()
window_size = Rect(0, 0, 691, 272)
screen = pygame.display.set_mode(window_size.size)
img = pygame.image.load("./files/keyboard3.jpg")
# initialize ros publisher
twist_pub = rospy.Publisher('keyboard/twist', Twist, queue_size=10)
joy_pub = rospy.Publisher('/joy', Joy, queue_size=10)
rospy.init_node('key2joy')
rate = rospy.Rate(50)
# init joy message
joy_ = Joy()
joy_.header.frame_id = 'map'
for i in range(8):
joy_.axes.append(0.0)
for i in range(11):
joy_.buttons.append(0)
while not rospy.is_shutdown():
rate.sleep()
screen.blit(img, (1,1))
pygame.display.flip()
# reset message axes
# for i in range(8):
# joy_.axes[i] = 0.0
#
# # reset buttons
# for i in range(11):
# joy_.buttons[i] = 0
for event in pygame.event.get():
# ---------------------- key dowm message ----------------------
if event.type == KEYDOWN:
# position control
if event.key == pygame.K_UP:
print 'forward'
joy_.axes[4] = 1.0
if event.key == pygame.K_DOWN:
print 'backward'
joy_.axes[4] = -1.0
if event.key == pygame.K_LEFT:
print 'left'
joy_.axes[3] = 1.0
if event.key == pygame.K_RIGHT:
print 'right'
joy_.axes[3] = -1.0
# yaw and z control
if event.key == pygame.K_w:
print 'up'
joy_.axes[1] = 1.0
if event.key == pygame.K_s:
print 'down'
joy_.axes[1] = -1.0
if event.key == pygame.K_a:
print 'turn left'
joy_.axes[0] = 1.0
if event.key == pygame.K_d:
print 'turn right'
joy_.axes[0] = -1.0
# task control
if event.key == pygame.K_n:
print 'clear'
joy_.buttons[6] = 1
if event.key == pygame.K_m:
print 'start'
joy_.buttons[7] = 1
joy_pub.publish(joy_)
# when keyup, reset velcity
elif event.type == pygame.KEYUP:
# position control
if event.key == pygame.K_UP:
# print 'forward'
joy_.axes[4] = 0.0
if event.key == pygame.K_DOWN:
# print 'backward'
joy_.axes[4] = -0.0
if event.key == pygame.K_LEFT:
# print 'left'
joy_.axes[3] = 0.0
if event.key == pygame.K_RIGHT:
# print 'right'
joy_.axes[3] = -0.0
# yaw and z control
if event.key == pygame.K_w:
# print 'up'
joy_.axes[1] = 0.0
if event.key == pygame.K_s:
# print 'down'
joy_.axes[1] = -0.0
if event.key == pygame.K_a:
# print 'turn left'
joy_.axes[0] = 0.0
if event.key == pygame.K_d:
# print 'turn right'
joy_.axes[0] = -0.0
# task control
if event.key == pygame.K_n:
# print 'start'
joy_.buttons[6] = 0
if event.key == pygame.K_m:
# print 'clear'
joy_.buttons[7] = 0
joy_pub.publish(joy_)
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass