def run(self): self.__mainLogger.debug('Controller - RUNNING') # Starts commands listener self.__cmdListener = CommandsListener(self, self.__mainLogger) self.__cmdListener.start() # Run gobject loop self.__loop = gobject.MainLoop() self.__loop.run()
def run(self): self.__mainLogger.debug('Controller - RUNNING') # Starts commands listener self.__cmdListener = CommandsListener(self, self.__mainLogger) self.__cmdListener.start() # Run gobject loop self.__loop = gobject.MainLoop() self.__loop.run()
class ThymioController(object): def __init__(self, mainLogger): self.__mainLogger = mainLogger self.__psValue = [0, 0, 0, 0, 0, 0, 0] self.__psGroundAmbiantSensors = [0, 0] self.__psGroundReflectedSensors = [0, 0] self.__psGroundDeltaSensors = [0, 0] self.__motorspeed = [0, 0] self.__realMotorSpeed = [0, 0] self.__color = [0, 0, 0, 0] self.__temperature = [0] self.__sound = [0] self.__performActionReq = threading.Condition() self.__rSensorsReq = False self.__temperatureReq = False self.__rGroundSensorsReq = False self.__wMotorspeedReq = False self.__rMotorspeedReq = False self.__wColorReq = False self.__stopThymioReq = False self.__killReq = False self.__soundReq = False self.__commandsListener = None self.__simulationStarted = threading.Event() self.__simulation = None self.__simLogger = None # Init the main loop dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) # Get stub of the Aseba network bus = dbus.SessionBus() # Create Aseba network asebaNetworkObject = bus.get_object('ch.epfl.mobots.Aseba', '/') self.__asebaNetwork = dbus.Interface(asebaNetworkObject, dbus_interface='ch.epfl.mobots.AsebaNetwork') # self.__mainLogger.debug('Aseba nodes: ' + str(self.__asebaNetwork.GetNodesList())) # Load the aesl file self.__asebaNetwork.LoadScripts(cl.AESL_PATH, reply_handler=self.__dbusEventReply, error_handler=self.__dbusError) # Schedules first run of the controller glib.idle_add(self.__execute) def setSimulation(self, simulation): if not self.__simulation: self.__simulation = simulation self.__simLogger = simulation.getLogger() self.__simulationStarted.set() def __dbusError(self, e): # there was an error on D-Bus, stop loop self.__simLogger.critical('dbus error: %s' % str(e) + "\nNow sleeping for 1 second and retrying...") time.sleep(1) raise Exception("dbus error") def __dbusEventReply(self): # correct replay on D-Bus, ignore pass def __dbusSendEventName(self, eventName, params): ok = False while not ok: try: self.__asebaNetwork.SendEventName(eventName, params, reply_handler=self.__dbusEventReply, error_handler=self.__dbusError) ok = True except: self.__simLogger.critical("Error during SEND EVENT NAME: " + eventName + " - " + str(params)) ok = False def __dbusGetVariable(self, varName, replyHandler): ok = False while not ok: try: self.__asebaNetwork.GetVariable("thymio-II", varName, reply_handler=replyHandler, error_handler=self.__dbusError) ok = True except: self.__simLogger.critical("Error during GET VARIABLE: " + varName) ok = False def __dbusSetMotorspeed(self): self.__dbusSendEventName("SetSpeed", self.__motorspeed) def __dbusSetColor(self): self.__dbusSendEventName("SetColor", self.__color) def __dbusSetSound(self): self.__dbusSendEventName("PlaySound", self.__sound) def __dbusGetProxSensorsReply(self, r): self.__psValue = r def __dbusGetProxSensors(self): self.__dbusGetVariable("prox.horizontal", self.__dbusGetProxSensorsReply) def __dbusGetTemperatureReply(self, r): self.__temperature = r def __dbusGetTemperature(self): self.__dbusGetVariable("temperature", self.__dbusGetTemperatureReply) def __dbusGetMotorSpeed(self): self.__dbusGetVariable("motor.left.speed ", self.__dbusGetLeftSpeedReply) self.__dbusGetVariable("motor.right.speed ", self.__dbusGetRightSpeedReply) def __dbusGetLeftSpeedReply(self, r): self.__realMotorSpeed[0] = r def __dbusGetRightSpeedReply(self, r): self.__realMotorSpeed[1] = r def __dbusGetGroundAmbiantReply(self, r): self.__psGroundAmbiantSensors = r def __dbusGetGroundReflectedReply(self, r): self.__psGroundReflectedSensors = r def __dbusGetGroundDeltaReply(self, r): self.__psGroundDeltaSensors = r def __dbusGetGroundSensors(self): self.__dbusGetVariable("prox.ground.ambiant", self.__dbusGetGroundAmbiantReply) self.__dbusGetVariable("prox.ground.reflected", self.__dbusGetGroundReflectedReply) self.__dbusGetVariable("prox.ground.delta", self.__dbusGetGroundDeltaReply) def __stopThymio(self): # Red LEDs: Thymio stops moving self.__sound = [1] self.__dbusSetSound() self.__motorspeed = [0, 0] self.__dbusSetMotorspeed() self.__simulation = None self.__simulationStarted.clear() def __execute(self): # Wait for the simulation to be set while not self.__simulationStarted.isSet(): self.__simulationStarted.wait() # Notifying that simulation has been set self.__simulation.thymioControllerPerformedAction() with self.__performActionReq: # Wait for requests: while not (self.__soundReq or self.__rSensorsReq or self.__rGroundSensorsReq or self.__temperatureReq or self.__wMotorspeedReq or self.__rMotorspeedReq or self.__wColorReq or self.__stopThymioReq or self.__killReq): self.__performActionReq.wait() if self.__rSensorsReq: # Read sensor values self.__dbusGetProxSensors() self.__rSensorsReq = False elif self.__rGroundSensorsReq: # Read ground sensor values self.__dbusGetGroundSensors() self.__rGroundSensorsReq = False elif self.__temperatureReq: # Read temperature values self.__dbusGetTemperature() self.__temperatureReq = False elif self.__soundReq: # emit sound self.__dbusSetSound() self.__soundReq = False elif self.__wMotorspeedReq: # Write motorspeed self.__dbusSetMotorspeed() # IF COMMENTED: wheels don't move # Make sure that Thymio moved for 1 timestep time.sleep( cl.TIME_STEP) # TODO: more precise -> thymio should notify controller when it moved for 50 ms self.__wMotorspeedReq = False elif self.__rMotorspeedReq: self.__dbusGetMotorSpeed() self.__rMotorspeedReq = False elif self.__wColorReq: self.__dbusSetColor() self.__wColorReq = False elif self.__stopThymioReq: # Stop Thymio self.__stopThymio() self.__stopThymioReq = False elif self.__killReq: # Kill everything self.__stopThymio() self.__loop.quit() return False return True def readTemperatureRequest(self): with self.__performActionReq: self.__temperatureReq = True self.__performActionReq.notify() def readSensorsRequest(self): with self.__performActionReq: self.__rSensorsReq = True self.__performActionReq.notify() def readGroundSensorsRequest(self): with self.__performActionReq: self.__rGroundSensorsReq = True self.__performActionReq.notify() def writeMotorspeedRequest(self, motorspeed): with self.__performActionReq: self.__motorspeed = motorspeed self.__wMotorspeedReq = True self.__performActionReq.notify() def readMotorspeedRequest(self): with self.__performActionReq: self.__rMotorspeedReq = True self.__performActionReq.notify() def writeColorRequest(self, color): with self.__performActionReq: self.__color = color self.__wColorReq = True self.__performActionReq.notify() def soundRequest(self, sound): with self.__performActionReq: self.__sound = sound self.__soundReq = True self.__performActionReq.notify() def stopThymioRequest(self): with self.__performActionReq: self.__stopThymioReq = True self.__performActionReq.notify() def killRequest(self): with self.__performActionReq: self.__killReq = True self.__performActionReq.notify() def getTemperature(self): return self.__temperature def getMotorSpeed(self): return self.__realMotorSpeed def getPSValues(self): return self.__psValue def getDeltaValues(self): return self.__psGroundDeltaSensors def getGroundSensorsValues(self): return self.__psGroundAmbiantSensors, self.__psGroundReflectedSensors, self.__psGroundDeltaSensors def run(self): self.__mainLogger.debug('Controller - RUNNING') # Starts commands listener self.__cmdListener = CommandsListener(self, self.__mainLogger) self.__cmdListener.start() # Run gobject loop self.__loop = gobject.MainLoop() self.__loop.run()
class ThymioController(object): def __init__(self, mainLogger): self.__mainLogger = mainLogger self.__psValue = [0, 0, 0, 0, 0, 0, 0] self.__psGroundAmbiantSensors = [0, 0] self.__psGroundReflectedSensors = [0, 0] self.__psGroundDeltaSensors = [0, 0] self.__motorspeed = [0, 0] self.__realMotorSpeed = [0, 0] self.__color = [0, 0, 0, 0] self.__temperature = [0] self.__sound = [0] self.__performActionReq = threading.Condition() self.__rSensorsReq = False self.__temperatureReq = False self.__rGroundSensorsReq = False self.__wMotorspeedReq = False self.__rMotorspeedReq = False self.__wColorReq = False self.__stopThymioReq = False self.__killReq = False self.__soundReq = False self.__commandsListener = None self.__simulationStarted = threading.Event() self.__simulation = None self.__simLogger = None # Init the main loop dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) # Get stub of the Aseba network bus = dbus.SessionBus() # Create Aseba network asebaNetworkObject = bus.get_object('ch.epfl.mobots.Aseba', '/') self.__asebaNetwork = dbus.Interface( asebaNetworkObject, dbus_interface='ch.epfl.mobots.AsebaNetwork') # self.__mainLogger.debug('Aseba nodes: ' + str(self.__asebaNetwork.GetNodesList())) # Load the aesl file self.__asebaNetwork.LoadScripts(cl.AESL_PATH, reply_handler=self.__dbusEventReply, error_handler=self.__dbusError) # Schedules first run of the controller glib.idle_add(self.__execute) def setSimulation(self, simulation): if not self.__simulation: self.__simulation = simulation self.__simLogger = simulation.getLogger() self.__simulationStarted.set() def __dbusError(self, e): # there was an error on D-Bus, stop loop self.__simLogger.critical( 'dbus error: %s' % str(e) + "\nNow sleeping for 1 second and retrying...") time.sleep(1) raise Exception("dbus error") def __dbusEventReply(self): # correct replay on D-Bus, ignore pass def __dbusSendEventName(self, eventName, params): ok = False while not ok: try: self.__asebaNetwork.SendEventName( eventName, params, reply_handler=self.__dbusEventReply, error_handler=self.__dbusError) ok = True except: self.__simLogger.critical("Error during SEND EVENT NAME: " + eventName + " - " + str(params)) ok = False def __dbusGetVariable(self, varName, replyHandler): ok = False while not ok: try: self.__asebaNetwork.GetVariable("thymio-II", varName, reply_handler=replyHandler, error_handler=self.__dbusError) ok = True except: self.__simLogger.critical("Error during GET VARIABLE: " + varName) ok = False def __dbusSetMotorspeed(self): self.__dbusSendEventName("SetSpeed", self.__motorspeed) def __dbusSetColor(self): self.__dbusSendEventName("SetColor", self.__color) def __dbusSetSound(self): self.__dbusSendEventName("PlaySound", self.__sound) def __dbusGetProxSensorsReply(self, r): self.__psValue = r def __dbusGetProxSensors(self): self.__dbusGetVariable("prox.horizontal", self.__dbusGetProxSensorsReply) def __dbusGetTemperatureReply(self, r): self.__temperature = r def __dbusGetTemperature(self): self.__dbusGetVariable("temperature", self.__dbusGetTemperatureReply) def __dbusGetMotorSpeed(self): self.__dbusGetVariable("motor.left.speed ", self.__dbusGetLeftSpeedReply) self.__dbusGetVariable("motor.right.speed ", self.__dbusGetRightSpeedReply) def __dbusGetLeftSpeedReply(self, r): self.__realMotorSpeed[0] = r def __dbusGetRightSpeedReply(self, r): self.__realMotorSpeed[1] = r def __dbusGetGroundAmbiantReply(self, r): self.__psGroundAmbiantSensors = r def __dbusGetGroundReflectedReply(self, r): self.__psGroundReflectedSensors = r def __dbusGetGroundDeltaReply(self, r): self.__psGroundDeltaSensors = r def __dbusGetGroundSensors(self): self.__dbusGetVariable("prox.ground.ambiant", self.__dbusGetGroundAmbiantReply) self.__dbusGetVariable("prox.ground.reflected", self.__dbusGetGroundReflectedReply) self.__dbusGetVariable("prox.ground.delta", self.__dbusGetGroundDeltaReply) def __stopThymio(self): # Red LEDs: Thymio stops moving self.__sound = [1] self.__dbusSetSound() self.__motorspeed = [0, 0] self.__dbusSetMotorspeed() self.__simulation = None self.__simulationStarted.clear() def __execute(self): # Wait for the simulation to be set while not self.__simulationStarted.isSet(): self.__simulationStarted.wait() # Notifying that simulation has been set self.__simulation.thymioControllerPerformedAction() with self.__performActionReq: # Wait for requests: while not (self.__soundReq or self.__rSensorsReq or self.__rGroundSensorsReq or self.__temperatureReq or self.__wMotorspeedReq or self.__rMotorspeedReq or self.__wColorReq or self.__stopThymioReq or self.__killReq): self.__performActionReq.wait() if self.__rSensorsReq: # Read sensor values self.__dbusGetProxSensors() self.__rSensorsReq = False elif self.__rGroundSensorsReq: # Read ground sensor values self.__dbusGetGroundSensors() self.__rGroundSensorsReq = False elif self.__temperatureReq: # Read temperature values self.__dbusGetTemperature() self.__temperatureReq = False elif self.__soundReq: # emit sound self.__dbusSetSound() self.__soundReq = False elif self.__wMotorspeedReq: # Write motorspeed self.__dbusSetMotorspeed() # IF COMMENTED: wheels don't move # Make sure that Thymio moved for 1 timestep time.sleep( cl.TIME_STEP ) # TODO: more precise -> thymio should notify controller when it moved for 50 ms self.__wMotorspeedReq = False elif self.__rMotorspeedReq: self.__dbusGetMotorSpeed() self.__rMotorspeedReq = False elif self.__wColorReq: self.__dbusSetColor() self.__wColorReq = False elif self.__stopThymioReq: # Stop Thymio self.__stopThymio() self.__stopThymioReq = False elif self.__killReq: # Kill everything self.__stopThymio() self.__loop.quit() return False return True def readTemperatureRequest(self): with self.__performActionReq: self.__temperatureReq = True self.__performActionReq.notify() def readSensorsRequest(self): with self.__performActionReq: self.__rSensorsReq = True self.__performActionReq.notify() def readGroundSensorsRequest(self): with self.__performActionReq: self.__rGroundSensorsReq = True self.__performActionReq.notify() def writeMotorspeedRequest(self, motorspeed): with self.__performActionReq: self.__motorspeed = motorspeed self.__wMotorspeedReq = True self.__performActionReq.notify() def readMotorspeedRequest(self): with self.__performActionReq: self.__rMotorspeedReq = True self.__performActionReq.notify() def writeColorRequest(self, color): with self.__performActionReq: self.__color = color self.__wColorReq = True self.__performActionReq.notify() def soundRequest(self, sound): with self.__performActionReq: self.__sound = sound self.__soundReq = True self.__performActionReq.notify() def stopThymioRequest(self): with self.__performActionReq: self.__stopThymioReq = True self.__performActionReq.notify() def killRequest(self): with self.__performActionReq: self.__killReq = True self.__performActionReq.notify() def getTemperature(self): return self.__temperature def getMotorSpeed(self): return self.__realMotorSpeed def getPSValues(self): return self.__psValue def getDeltaValues(self): return self.__psGroundDeltaSensors def getGroundSensorsValues(self): return self.__psGroundAmbiantSensors, self.__psGroundReflectedSensors, self.__psGroundDeltaSensors def run(self): self.__mainLogger.debug('Controller - RUNNING') # Starts commands listener self.__cmdListener = CommandsListener(self, self.__mainLogger) self.__cmdListener.start() # Run gobject loop self.__loop = gobject.MainLoop() self.__loop.run()