class Interface:
def __init__(self, robothost, robotport):
self.ctr = Control(self.__class__.__name__)
self.robot = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
while True:
try:
self.robot.connect((robothost, robotport))
except socket.error:
print "NO CONNECTION TO THE ROBOT, WTF ARE YOU DOING!"
sleep(5)
continue
break
self.mainloop()
exit(0)
def mainloop(self):
#Spawn Bro-bot
self.robot.send(ROBOT_COMMAND)
while True:
recv = self.ctr.receive(True)
if recv:
src, data = recv
if src == 'main' and data == 'STOP':
break
self.robot.send(data)
ready, _, _ = select(list(self.robot),(),(), 0.05)
for x in ready:
data = x.recv(1024)
self.ctr.send('Sensors', data)
class Alert:
#Initialises control class, and tresholds
def __init__(self):
self.ctrl = Control(self.__class__.__name__)
self.TRESHOLD_S = 0.4
self.TRESHOLD_L = 0.25
self.RECOVERTIME = 3
self.running = True
self.run()
# Alternate between checkin Laser and Sonar distance
def run(self):
while self.running:
self.check("LASER", self.TRESHOLD_L)
self.check("SONAR", self.TRESHOLD_S)
# Get sensor values, compare each to treshold, send out ALERT if below
def check(self, sensor, treshold):
data = ""
self.ctrl.send("Sensors", "GET " + sensor)
while not data[:5] == sensor:
data = self.ctrl.receive()
if data == None:
break
else:
src, data = data
if src == "main" and data == "STOP":
self.running = False
exit()
vals = data[6:].split(' ')
# Send out alert & sleep when on collision course
for it in vals:
if it < treshold:
send("Steering", "ALERT")
send("Logic", "ALERT")
sleep(self.RECOVERTIME)
continue
class Sensors:
# Init
def __init__(self):
self.ctrl = Control(self.__class__.__name__)
self.data = ""
self.sensors = {"LASER" : "", # "SENS LASER"
"ODOMETRY" : "", # "SENS ODOMETRY <float x> <float y> <float z>"
"SONAR" : ""} # "SENS SONAR <float F1> <float F2> <float F3> <float F4> <float F5> <float F6> <float F7> <float F8>"
self.running = True
self.receive()
# Receive and store data
def receive(self):
while(self.running):
rcv = self.ctrl.receive(True)
if rcv == None:
break
else:
src, rcv = rcv
# Mine data from Bot stream
if src == "Interface":
self.data = self.data + rcv
self.data = self.data.split('\r\n')
for i in range(len(self.data) - 1):
# LASER
if not self.data[i].find("Scanner1"):
self.sensors["LASER"] = "LASER " + self.data[i].split(' ')[12].replace(',', ' ')
# ODOMETER
elif not self.data[i].find("Odometry"):
temp = self.data[i].split(' ')
vals = temp[6].split(',')
self.sensors["ODOMETRY"] = "ODOMETRY " + vals[0] + vals[1] + vals[2].split('}')[0]
# SONAR
elif not self.data[i].find("Sonar"):
temp = self.data[i].split(' ')
string = "SONAR"
# XXX: Not sure if OK
for i in range(8):
val = temp[5 * i + 8].split('}')[0]
string = string + ' ' + val
self.sensors["SONAR"] = string
self.data = self.data[len(self.data) - 1]
# Respond to msges from main (STOP, RESTART)
elif src == "Main":
#
if rcv == "RESTART":
self.running = False
self.reset()
if rcv == "STOP":
self.running = False
# Reply to GET by sending last known values
else:
rcv = rcv.split(' ')
if rcv[0] == "GET":
# XXX: if x in y instead of try?
try:
self.ctrl.send(src + ' ' + self.sensors[rcv[1]] )
except:
self.ctrl.send(src + ' ' + rcv[1] + " FAIL" )
# Well obviously...
def reset(self):
self.data = ""
for i in self.sensors.keys():
self.sensors[i] = ""
self.running = True
self.receive()
class Driverandom:
def __init__(self):
self.ctrl = Control(self.__class__.__name__)
self.gridSize = 1.0
self.treshold_l = 0.4
self.treshold_s = 0.25
self.sonar = ""
self.laser = ""
self.running = True
self.drive(0)
# Retrieves sensor data from the sensor module
def getData(self, sensor):
self.ctrl.send("Sensors", "GET " + sensor)
data = None
while data == None or data[0] != "Sensors" or (data[0] == "Sensors" and data[1][:5] != sensor):
data = self.ctrl.receive()
if data != None:
src, data = data
if src == "main" and data == "STOP":
self.running = False
exit()
return data[6:].split(' ')
def getGridSize(self):
return 1.0
# Returns a random amount of grid steps to take, from within a collision
# free range
def getSteps(self, dist):
# XXX If stupid just floor() (or int())
dist_norm = ceil(dist) if (dist % floor(dist)) > 0.9 else floor(dist)
return random.randrange(1, dist_norm)
#Drives a random path, based on a given seed
def drive(self, seed):
random.seed(seed)
while(self.running):
self.getGridSize()
turn_deg = random.randrange(0,4,1)
self.ctrl.send("Steering", "turn 1 " + str(turn_deg * 90))
self.sonar = self.getData("SONAR")
self.laser = self.getData("LASER")
# TODO: Laser more accurate
# Get current sensor data
laserFrontDistance = float(self.laser[90]) - self.treshold_l
sonarFrontDistance = (float(self.sonar[3]) + float(self.sonar[4]))/2 - self.treshold_s
# The 1.5 is arbitrary, need to experiment for perfect value (if it
# exists)
if laserFrontDistance - sonarFrontDistance > 1.5:
steps = self.getSteps(sonarFrontDistance)
elif sonarFrontDistance - laserFrontDistance > 1.5:
steps = self.getSteps(laserFrontDistance)
else:
steps = self.getSteps((laserFrontDistance + sonarFrontDistance) / 2)
self.ctrl.send("Steering", "Move 1 " + steps / self.gridSize)
