def onStart(self): # Set up socket for emitting fake tag messages s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP) s.bind(("", 0)) self.sock = s # Set up the sensor receiver plan self.sensor = SensorPlanTCP(self, server=self.srvAddr[0]) self.sensor.start() self.timeForStatus = self.onceEvery(1) self.timeForLaser = self.onceEvery(1 / 15.0) self.timeForFrame = self.onceEvery(1 / 20.0) self.timeForFilter = self.onceEvery(1.0 / 10.0) progress("Using %s:%d as the waypoint host" % self.srvAddr) self.T0 = self.now # Setup autonomous mode timer self.timeForAuto = self.onceEvery(1) self.auto = False self.core = Core(Mode.SIMULATION, self) self.robSim = RedRobotSim(self.core, fn=None) self.core.setSim(self.robSim) self.core.start() self.startedFilter = False
def onStart( self ): # Set up socket for emitting fake tag messages s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP) s.bind(("",0)) self.sock = s # Set up the sensor receiver plan self.sensor = SensorPlanTCP(self,server=self.srvAddr[0]) self.sensor.start() self.timeForStatus = self.onceEvery(1) self.timeForLaser = self.onceEvery(1/15.0) self.timeForFrame = self.onceEvery(1/20.0) self.timeForFilter = self.onceEvery(1.0/10.0) progress("Using %s:%d as the waypoint host" % self.srvAddr) self.T0 = self.now # Setup autonomous mode timer self.timeForAuto = self.onceEvery(1) self.auto = False self.core = Core(Mode.SIMULATION, self) self.robSim = RedRobotSim(self.core, fn=None) self.core.setSim(self.robSim) self.core.start() self.startedFilter = False
class RobotSimulatorApp(JoyApp): """Concrete class RobotSimulatorApp <<singleton>> A JoyApp which runs the RedRobotSim robot model in simulation, and emits regular simulated tagStreamer message to the desired waypoint host. Used in conjection with waypointServer.py to provide a complete simulation environment for Project 1 """ def __init__(self, wphAddr=WAYPOINT_HOST, *arg, **kw): JoyApp.__init__( self, confPath="$/cfg/JoyApp.yml", ) self.srvAddr = (wphAddr, APRIL_DATA_PORT) self.auto = False def onStart(self): # Set up socket for emitting fake tag messages s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP) s.bind(("", 0)) self.sock = s # Set up the sensor receiver plan self.sensor = SensorPlanTCP(self, server=self.srvAddr[0]) self.sensor.start() self.timeForStatus = self.onceEvery(1) self.timeForLaser = self.onceEvery(1 / 15.0) self.timeForFrame = self.onceEvery(1 / 20.0) self.timeForFilter = self.onceEvery(1.0 / 10.0) progress("Using %s:%d as the waypoint host" % self.srvAddr) self.T0 = self.now # Setup autonomous mode timer self.timeForAuto = self.onceEvery(1) self.auto = False self.core = Core(Mode.SIMULATION, self) self.robSim = RedRobotSim(self.core, fn=None) self.core.setSim(self.robSim) self.core.start() self.startedFilter = False # self.logFile = open("log.txt", "w") def showSensors(self): ts, f, b = self.sensor.lastSensor if ts: progress("Sensor: %4d f %d b %d" % (ts - self.T0, f, b)) else: progress("Sensor: << no reading >>") ts, w = self.sensor.lastWaypoints if ts: progress("Waypoints: %4d " % (ts - self.T0) + str(w)) else: progress("Waypoints: << no reading >>") def emitTagMessage(self): """Generate and emit and update simulated tagStreamer message""" self.robSim.refreshState() # Get the simulated tag message msg = self.robSim.getTagMsg() # Send message to waypointServer "as if" we were tagStreamer self.sock.sendto(msg, self.srvAddr) def onEvent(self, evt): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() print( "CurrPos(Camera): " + str( self.core.coordinateFrames.convertRealToCamera( self.robSim.pos))) print("CurrPos(Real): " + str(self.robSim.pos)) try: # print("Estimated Pos(Real): " + str(self.core.particleFilter.getState().pos) + "\t" + str(self.core.particleFilter.getState().yaw)) print( "CurrPos(waypts): " + str( self.core.coordinateFrames.convertRealToWaypoint( self.robSim.pos)) + "\t" + str(self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw())) print("EstimatedPos(waypts): " + str(self.core.particleFilter.getWaypointState().pos) + "\t" + str(self.core.particleFilter.getWaypointState().yaw)) except: pass progress(self.robSim.logLaserValue(self.now)) # logging # if (self.core.coordinateFrames.waypoint != None): # rotatedLength = CoordinateFrames.rotateCCW([Constants.tagLength, 0], self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw()) # posWaypoint = self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos) # frontSensor = posWaypoint[1] + rotatedLength[1] # backSensor = posWaypoint[1] - rotatedLength[1] # if (self.sensor.lastSensor[1] > 5 and self.sensor.lastSensor[2] > 5): # self.logFile.write(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]])) # self.logFile.write(str([frontSensor, backSensor]) + "\n") # print(rotatedLength) # print(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]) + str([frontSensor, backSensor])) # generate simulated laser readings elif self.timeForLaser(): self.robSim.logLaserValue(self.now) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() if self.timeForFilter(): self.core.setSensorAndWaypoints( array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]), self.sensor.lastWaypoints[1]) if self.timeForAuto() and self.auto: self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1]) if evt.type == KEYDOWN: if evt.key == K_UP: self.core.movePosY() return progress("(say) Up") elif evt.key == K_DOWN: self.core.moveNegY() return progress("(say) Down") elif evt.key == K_LEFT: self.core.moveNegX() return progress("(say) Left") elif evt.key == K_RIGHT: self.core.movePosX() return progress("(say) Right") elif evt.key == K_RETURN: self.startedFilter = not self.startedFilter if (self.startedFilter): self.core.startFilter() return progress("(say) Started Filter") else: self.core.stopFilter() return progress("(say) Stopped Filter") elif evt.key == K_TAB: self.auto = ~self.auto if self.auto: return progress("(say) Autonomous On") else: return progress("(say) Autonomous Off") else: return JoyApp.onEvent(self, evt) return # ignoring non-KEYDOWN events
class RobotSimulatorApp( JoyApp ): """Concrete class RobotSimulatorApp <<singleton>> A JoyApp which runs the RedRobotSim robot model in simulation, and emits regular simulated tagStreamer message to the desired waypoint host. Used in conjection with waypointServer.py to provide a complete simulation environment for Project 1 """ def __init__(self,wphAddr=WAYPOINT_HOST,*arg,**kw): JoyApp.__init__( self, confPath="$/cfg/JoyApp.yml", ) self.srvAddr = (wphAddr, APRIL_DATA_PORT) self.auto = False; def onStart( self ): # Set up socket for emitting fake tag messages s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP) s.bind(("",0)) self.sock = s # Set up the sensor receiver plan self.sensor = SensorPlanTCP(self,server=self.srvAddr[0]) self.sensor.start() self.timeForStatus = self.onceEvery(1) self.timeForLaser = self.onceEvery(1/15.0) self.timeForFrame = self.onceEvery(1/20.0) self.timeForFilter = self.onceEvery(1.0/10.0) progress("Using %s:%d as the waypoint host" % self.srvAddr) self.T0 = self.now # Setup autonomous mode timer self.timeForAuto = self.onceEvery(1) self.auto = False self.core = Core(Mode.SIMULATION, self) self.robSim = RedRobotSim(self.core, fn=None) self.core.setSim(self.robSim) self.core.start() self.startedFilter = False # self.logFile = open("log.txt", "w") def showSensors( self ): ts,f,b = self.sensor.lastSensor if ts: progress( "Sensor: %4d f %d b %d" % (ts-self.T0,f,b) ) else: progress( "Sensor: << no reading >>" ) ts,w = self.sensor.lastWaypoints if ts: progress( "Waypoints: %4d " % (ts-self.T0) + str(w)) else: progress( "Waypoints: << no reading >>" ) def emitTagMessage( self ): """Generate and emit and update simulated tagStreamer message""" self.robSim.refreshState() # Get the simulated tag message msg = self.robSim.getTagMsg() # Send message to waypointServer "as if" we were tagStreamer self.sock.sendto(msg, self.srvAddr) def onEvent( self, evt ): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() print("CurrPos(Camera): " + str(self.core.coordinateFrames.convertRealToCamera(self.robSim.pos))) print("CurrPos(Real): " + str(self.robSim.pos)) try: # print("Estimated Pos(Real): " + str(self.core.particleFilter.getState().pos) + "\t" + str(self.core.particleFilter.getState().yaw)) print("CurrPos(waypts): " + str(self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos)) + "\t" + str(self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw())) print("EstimatedPos(waypts): " + str(self.core.particleFilter.getWaypointState().pos) + "\t" + str(self.core.particleFilter.getWaypointState().yaw)) except: pass progress( self.robSim.logLaserValue(self.now) ) # logging # if (self.core.coordinateFrames.waypoint != None): # rotatedLength = CoordinateFrames.rotateCCW([Constants.tagLength, 0], self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw()) # posWaypoint = self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos) # frontSensor = posWaypoint[1] + rotatedLength[1] # backSensor = posWaypoint[1] - rotatedLength[1] # if (self.sensor.lastSensor[1] > 5 and self.sensor.lastSensor[2] > 5): # self.logFile.write(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]])) # self.logFile.write(str([frontSensor, backSensor]) + "\n") # print(rotatedLength) # print(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]) + str([frontSensor, backSensor])) # generate simulated laser readings elif self.timeForLaser(): self.robSim.logLaserValue(self.now) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() if self.timeForFilter(): self.core.setSensorAndWaypoints(array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]), self.sensor.lastWaypoints[1]) if self.timeForAuto() and self.auto: self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1]) if evt.type == KEYDOWN: if evt.key == K_UP: self.core.movePosY() return progress("(say) Up") elif evt.key == K_DOWN: self.core.moveNegY() return progress("(say) Down") elif evt.key == K_LEFT: self.core.moveNegX() return progress("(say) Left") elif evt.key == K_RIGHT: self.core.movePosX() return progress("(say) Right") elif evt.key == K_RETURN: self.startedFilter = not self.startedFilter if (self.startedFilter): self.core.startFilter() return progress("(say) Started Filter") else: self.core.stopFilter() return progress("(say) Stopped Filter") elif evt.key == K_TAB: self.auto = ~self.auto if self.auto: return progress("(say) Autonomous On") else: return progress("(say) Autonomous Off") else: return JoyApp.onEvent(self,evt) return # ignoring non-KEYDOWN events