-
Notifications
You must be signed in to change notification settings - Fork 1
/
e_manual_control.py
98 lines (75 loc) · 2.37 KB
/
e_manual_control.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
from mir import MirManual
import curses
def log(string, line, screen):
screen.addstr(line, 0, string)
screen.clrtoeol()
screen.refresh()
def start_controlling(mir, screen):
mir.callManualMode()
mir.subToSafetyInfo()
# waiting for data from mir
while not hasattr(mir, 'safetyInfo'):
pass
log("Press blue button on robot to unlock manual mode.", 0, screen)
# waiting for button press
while mir.safetyInfo['is_manual_mode_restart_required']:
pass
log("Now use the arrowkeys to move robot", 0, screen)
log("Press '+' or '-' to set speed limit.", 1, screen)
log("Press 'q' to quit.", 2, screen)
x = 0
z = 0
while True:
arrow_key = curses.initscr().getch()
if arrow_key == ord('q'):
break
if arrow_key == ord('+'):
mir.callSpeedLimit(False)
if arrow_key == ord('-'):
mir.callSpeedLimit(True)
if arrow_key == curses.KEY_UP:
x = 0.3
elif arrow_key == curses.KEY_DOWN:
x = -0.3
else: x = 0
if arrow_key == curses.KEY_RIGHT:
z = -0.3
elif arrow_key == curses.KEY_LEFT:
z = 0.3
else: z = 0
mir.move(x, z)
def end_controlling(mir, screen):
# Close websocket connection to mir
mir.callPauseMode()
mir.terminate()
# Restore terminal
screen.keypad(False)
curses.nocbreak()
curses.echo()
curses.endwin()
def main():
session_id = '03bdbug5tprmn0q7l00b742vh7' # extracted from web-interace
sdt_host = "mir.com"
print(f"Enter mir's network-address: (hit enter to use '{sdt_host}')")
usr_host = input()
print("OK, Trying to connect to MiR...")
# Connecting to MiR
host = usr_host if len(usr_host) > 0 else sdt_host
mir = MirManual(host, 9090, session_id)
try: mir.connect()
except:
print("Cloud not connect to mir! Leaving...")
return
print("Connected!")
print("Starting user interface for manual control...\n")
# Setting up user-interface
screen = curses.initscr()
screen.keypad(True)
curses.noecho()
curses.cbreak()
try: start_controlling(mir, screen)
finally:
# Making sure the connection to MIR is always closed savely
end_controlling(mir, screen)
print("Back to terminal and closed connection to mir safely.")
main()