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)