-
Notifications
You must be signed in to change notification settings - Fork 0
/
roboslave.py
executable file
·131 lines (115 loc) · 4.19 KB
/
roboslave.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
127
128
129
130
#!/usr/bin/python
# Roboslave - run the robot in remotely-controlled slave mode
import sys,threading,traceback,time, SocketServer
import robocamera,robosensors,robopower,robonavigation,roboconfig
import robocomms
from robocomms import log
from roboutils import *
#-------------------------------------------------------------------
# Log the robot status
#-------------------------------------------------------------------
def log_robot_status():
ping = robosensors.get_sensors()
touch_sensor = robosensors.get_bumper()
log(">> Speed=%d" % robopower.get_power())
log(">> Compass=%3.2f" % robosensors.get_compass())
log(">> Touch=%s, Ping1=%d, Ping2=%d, Ping3=%d" % \
(touch_sensor, ping[0], ping[1], ping[2]))
#-------------------------------------------------------------------
# Robot initialization
#
# Call the intialize() function on each of the robot subsystems. If
# the subsystems run into problems they will throw a RobotError which
# will be caught here to terminate processing.
#
def initialize():
try:
robocomms.initialize()
robopower.initialize()
robocamera.initialize()
robosensors.initialize()
robonavigation.initialize()
except RobotError as e:
log("RobotException: " + e.value)
log("Exception occurred in initialization - ending")
robopower.halt()
exit(0)
return
#-------------------------------------------------------------------------
# Process the received command
#-------------------------------------------------------------------------
def process_command(command):
# Ignore blank lines
if command == "":
return
# Speed command
if command[0:5].lower() == 'speed':
speed_re = re.compile('speed\s+([+|-]*)(\d+)')
match = speed_re.search(command)
if not match:
log("Invalid speed command, usage is: speed <integer>")
return
speed = int("%s%s" % (match.group(1), match.group(2)))
if speed < -16 or speed > 16:
log("Invalid speed: must be -16 <= speed <= 16")
return
robopower.speed(speed)
log("Speed set to %d" % speed)
return
# Stop command
if command[0:4].lower() == 'stop':
robopower.stop()
log("Robot stopped")
return
# No match - must be an invalid command
log("Invalid command: %s" % command)
#-------------------------------------------------------------------------
# UDP Server
#-------------------------------------------------------------------------
class MyUDPHandler(SocketServer.BaseRequestHandler):
# Handle incoming data
def handle(self):
data = self.request[0].strip()
process_command(data)
log_robot_status()
#-------------------------------------------------------------------------
# Main processing loop
#
# Start the UDP server. This server will handle incoming commands.
#-------------------------------------------------------------------------
def mainloop():
log("Starting command server")
HOST = get_local_ipv4_address()
PORT = roboconfig.command_port
command_server = SocketServer.UDPServer((HOST,PORT),MyUDPHandler)
command_server.serve_forever()
#-----------------------------------------------------------------------
# Main
#
# Call initialize to start the robot. Then call mainloop() inside a
# try statement - if error occur, catch the exception and stop the
# robot before terminating the program.
#
# On exiting (cleanly or otherwise) terminate all threads that were
# created at initialization time.
#
def main():
log("RoboMagellan 2012 started")
initialize()
time.sleep(1) # Allow time for processing threads to start
try:
mainloop()
except:
log("*** ERROR *** A fatal error has occured *** ERROR ***")
log(str(traceback.format_exc()))
else:
log("RoboMagellan 2012 ended - Champagne Time!")
# Stop the robot
robopower.halt()
# Terminate running threads
for thread in threading.enumerate():
if thread.name != 'MainThread':
log("Terminating thread: " + str(thread))
thread.stop()
if __name__ == '__main__':
main()