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)
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()
def process_control_line(line):
"""Processes a line from the RoboMagellen control file."""
# skip comments and blank lines
if line[0] == '#':
return None
if line == "":
return None
re_anything = re.compile('\S+')
if re_anything.match(line) == None:
return None
# determine and process line type
line = line.rstrip()
if line[0:4] == "DRIV":
return process_driv_directive(line)
elif line[0:4] == "GOTO":
return process_goto_directive(line)
elif line[0:4] == "CONE":
return process_cone_directive(line)
elif line[0:4] == "STOP":
return process_stop_directive(line)
elif line[0:4] == "STER":
return process_ster_directive(line)
elif line[0:4] == "POWR":
return process_powr_directive(line)
elif line[0:4] == "WAIT":
return process_wait_directive(line)
else:
log("Undefined directive " + line[0:4] +" in line: " + line)
return False
def initialize():
log("Sensor initialization started")
compass_thread = CompassThread()
compass_thread.start()
sensor_thread = SensorThread()
sensor_thread.start()
log("Sensor initialization completed")
return True
def run(self):
while self.run_flag == True:
global compass_data
self.ser.flushInput() # Discard readings buffered during sleep
data = self.ser.readline()
m = self.compass_re.match(data)
if m:
compass_data = int(m.group(1)) + 0.1 * int(m.group(2))
else:
log("robosensors - recieved invalid compass data: %s" % data)
time.sleep(roboconfig.compass_read_delay)
log("Compass thread terminated")
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
def process_control_file(filename):
"""Parses the RoboMagellan control file. Returns a data structure with
the route details if sucessful, otherwise NULL if an error is
found"""
log("Processing control file: " + filename)
error_count = 0
control_data = []
# Process the file
f = open(filename)
while True:
line = f.readline()
log(">>> " + line.rstrip())
if not line: break
result = process_control_line(line)
if result == False:
error_count = error_count + 1
elif result == None:
pass
else:
control_data.append(result)
f.close()
# Return the control data or None to the caller
if error_count == 1:
log("Control file processed, " + str(error_count) + " error.")
else:
log("Control file processed, " + str(error_count) + " errors.")
if error_count == 0:
return control_data
else:
raise RobotError('Control file error')
def initialize():
try:
control_data = robocontrol.process_control_file(sys.argv[1])
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 control_data
def run(self):
while self.run_flag == True:
global ping_data, bumper_status
self.ser.flushInput() # Discard readings buffered during sleep
self.ser.readline()
# Read five lines of data to get complete input
for i in range(5):
data = self.ser.readline().lower().strip()
if data.find('bumper') == 0:
if data.find('on') > 0:
bumper_status = True
elif data.find('off') > 0:
bumper_status = False
else:
log("robosensors - recieved invalid bumper data: %s" % \
data)
elif data.find('sensor') == 0:
m = self.sensor_re.match(data)
if m:
ping_data[int(m.group(1))-1] = int(m.group(2))
else:
log("robosensors - recieved invalid sensor data: %s" % \
data)
else:
log("robosensor - received invalid data %s" % data)
time.sleep(roboconfig.sensor_read_delay)
log("Sensor thread terminated")
def set_speed(self, k):
k_val = 17 - abs(k)
if k < 0:
right_motor = "1,0,1,%s\n" % k_val
left_motor = "2,0,0,%s\n" % k_val
elif k == 0:
right_motor = "1,1,0,0\n"
left_motor = "2,1,0,0\n"
else:
right_motor = "1,0,0,%s\n" % k_val
left_motor = "2,0,1,%s\n" % k_val
log("Right: %s" % right_motor)
log("Left: %s" % left_motor)
self.ser.write(right_motor)
self.ser.write(left_motor)
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()
def run(self):
while True:
global current_speed, target_speed
# If necessary, bring current speed to target
if current_speed != target_speed:
if current_speed < target_speed:
values = range(current_speed+1, target_speed+1)
else:
values = range(current_speed-1, target_speed-1, -1)
for k in values:
log("Setting speed to %d" % k)
self.set_speed(k)
current_speed = k
time.sleep(roboconfig.power_delay)
if self.run_flag == False:
break
time.sleep(roboconfig.power_delay)
log("Power thread terminated")
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]))
def process_cone_directive(line):
# Parse Cone data
cone_re = re.compile('CONE\s+(\d\d)(\d\d).(\d+)\s+(\d\d\d)(\d\d).(\d+)')
m = cone_re.search(line)
if not m:
log("Invalid CONE directive in line: " + line)
return False
# Validate and calculate latitude
lat_deg = int(m.group(1))
lat_min = int(m.group(2))
if lat_min > 59:
log("Minutes must be less than 60 in latitude specification in line: " + line)
return False
lat_min = lat_min + int(m.group(3))/1000.0
lat_deg = lat_deg + lat_min/60.0
# Validate and calculate longitude
lon_deg = int(m.group(4))
lon_min = int(m.group(5))
if lon_min > 59:
log("Minutes must be less than 60 in longitude specification in line: " + line)
return False
lon_min = lon_min + int(m.group(6))/1000.0
lon_deg = lon_deg + lon_min/60.0
return {'command': "CONE", 'latitude': lat_deg, 'longitude': lon_deg}
def mainloop(control_data):
for command in control_data:
log("Processing command: " + repr(command))
if command['command'] == 'CONE':
navigate_to(command['latitude'], command['longitude'], mode='cone')
elif command['command'] == 'GOTO':
navigate_to(command['latitude'], command['longitude'], mode='goto')
elif command['command'] == 'DRIV':
log("DRIV command not yet implemented")
elif command['command'] == 'STOP':
robopower.stop()
return
elif command['command'] == 'POWR':
robopower.power(command['power'])
elif command['command'] == 'STER':
robopower.steer(command['steering'])
elif command['command'] == 'WAIT':
time.sleep(command['sleeptime'])
else:
# Should never happen: robocontrol should catch, but hey...
log("Error in mainloop: invalid command " + command['command'])
robopower.halt()
raise RobotError
def re_sight_cone():
log("Re-sighting cone...")
# Try a few times
for i in range(roboconfig.re_sight_attempts):
(camera_X, camera_Y, camera_distance) = robocamera.get_data()
if camera_distance < roboconfig.max_camera_distance:
log("...sucess!")
return True
else:
time.sleep(re_sight_sleep_time)
# Couldn't find it - return False
log("...failed")
return False
def speed(speed_value):
assert(-16 <= speed_value <= 16)
global target_speed
log("Old target speed = %d" % target_speed)
target_speed = speed_value
log("New target speed = %d" % target_speed)
def stop(self):
log("Terminating sensor thread")
self.run_flag = False
def stop(self):
log("Terminating compass thread")
self.run_flag = False
def navigate_to(latitude, longitude, mode='cone'):
assert (mode == 'cone') or (mode == 'goto')
log("Navigate to lat: " + str(latitude) + ", lon: " + str(longitude))
log("Navigate to mode=" + mode)
task_complete = False
while task_complete == False: #TODO - probably need to time out
# Get camera, compass heading, and GPS data
(camera_X, camera_Y, camera_distance) = robocamera.get_data()
log("Navigate to: camera_X = %03d, camera_Y = %03d, distance = %1.2f" \
% (camera_X, camera_Y, camera_distance))
current_heading = robosensors.get_compass()
log("Navigate to: current heading = %d" % current_heading)
(gps_distance, heading_to_target) = \
robonavigation.vector_to_target(latitude, longitude)
log("Navigate to: target distance = %2.2f, vector = %d" % \
(gps_distance, heading_to_target))
# Test to see if we are at the GPS waypoint (within the radius of
# error). We we are and we're in goto mode, we're done. Otherwise
# switch to looking for the cone with the camera.
if gps_distance < roboconfig.gps_radius:
log("GPS waypoint found")
if mode == 'goto':
log("Goto mode - task complete")
return
else:
# If we can see the cone at this point, go and locate
# it. If we can't, keep going until we can see it.
#
# Note that locate_cone() may fail if the camera loses
# sight of the cone. If that happens, we drop back to
# locating the GPS waypoint again
if camera_distance < roboconfig.max_camera_distance:
if locate_cone() == True:
return
# Outside of GPS range - set the steer angle and drive/turn
# towards the cone
steer_angle = \
robonavigation.get_steer_angle(current_heading, heading_to_target)
log("Navigate to: steer_angle = " + str(steer_angle))
robopower.speed(5)
robopower.steer(steer_angle/180.0)
# Slow things down!
time.sleep(3)
def initialize():
log("Power initialization started")
power_thread = PowerThread()
power_thread.start()
log("Power initialization completed")
return True
