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):
# Init sensor plan
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
# Load plans here
self.moveP = MovePlan(self)
self.turnP = RotatePlan(self)
self.turretP = TurretPlan(self)
self.autoP = AutoPlan(self, self.sensor, self.moveP, self.turnP)
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.robSim = DummyRobotSim(fn=None)
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1 / 15.0)
self.timeForFrame = self.onceEvery(1 / 20.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
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):
self.xMotors = [self.robot.at.Nx04, self.robot.at.Nx02]
self.yMotors = [self.robot.at.Nx06, self.robot.at.Nx08]
self.motorPlan = MotorPlan(self)
self.core = Core(Mode.ACTUAL, self)
self.core.start()
self.timeForFilter = self.onceEvery(1.0 / 10.0)
self.startedFilter = False
self.auto = False
self.timeForServoMeasure = self.onceEvery(1.0 / 2.0)
#sensor stuff
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
# Setup autonomous mode timer
self.timeForAuto = self.onceEvery(10)
def onStart( self ):
"""
Sets up the JoyApp and configures the simulation
"""
### DO NOT MODIFY ------------------------------------------
# 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)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
### MODIFY FROM HERE ------------------------------------------
self.robSim = SimpleRobotSim(fn=None)
self.yMove = yMovePlan(self,self.robSim)
self.xMove = xMovePlan(self,self.robSim)
self.auto = Auto(self, self. robSim, self.yMove, self.xMove)
def onStart(self):
self.xMotors = [self.robot.at.Nx04, self.robot.at.Nx02]
self.yMotors = [self.robot.at.Nx06, self.robot.at.Nx08]
self.motorPlan = MotorPlan(self)
self.core = Core(Mode.ACTUAL, self)
self.core.start()
self.timeForFilter = self.onceEvery(1.0/10.0)
self.startedFilter = False
self.auto = False
self.timeForServoMeasure = self.onceEvery(1.0/2.0)
#sensor stuff
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
# Setup autonomous mode timer
self.timeForAuto = self.onceEvery(10)
class RobotSimulatorApp(JoyApp):
"""Concrete class RobotSimulatorApp <<singleton>>
A JoyApp which runs the DummyRobotSim 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,
wphPort=WAYPOINT_MSG_PORT,
*arg,
**kw):
"""
Initialize the simulator
"""
JoyApp.__init__(self, confPath="$/cfg/JoyApp.yml", *arg, **kw)
self.srvAddr = (wphAddr, wphPort)
# ADD pre-startup initialization here, if you need it
def onStart(self):
"""
Sets up the JoyApp and configures the simulation
"""
### DO NOT MODIFY ------------------------------------------
# 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],
port=self.srvAddr[1])
self.sensor.start()
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1 / 15.0)
self.timeForFrame = self.onceEvery(1 / 20.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
### MODIFY FROM HERE ------------------------------------------
self.robSim = SimpleRobotSim(fn=None)
self.moveP = MoveForward(self, self.robSim)
self.turnP = Turn(self, self.robSim)
def showSensors(self):
"""
Display sensor readings
"""
# This code should help you understand how you access sensor information
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"""
#### DO NOT MODIFY --- it WILL break the simulator
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.encode("ascii"),
(self.srvAddr[0], APRIL_DATA_PORT))
def onEvent(self, evt):
#### DO NOT MODIFY --------------------------------------------
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
progress(self.robSim.logLaserValue(self.now))
# generate simulated laser readings
elif self.timeForLaser():
self.robSim.logLaserValue(self.now)
# update the robot and simulate the tagStreamer
if self.timeForFrame():
self.emitTagMessage()
#### MODIFY FROM HERE ON ----------------------------------------
if evt.type == KEYDOWN:
if evt.key == K_UP and not self.moveP.isRunning():
self.moveP.dist = 100.0
self.moveP.start()
return progress("(say) Move forward")
elif evt.key == K_DOWN and not self.moveP.isRunning():
self.moveP.dist = -100.0
self.moveP.start()
return progress("(say) Move back")
if evt.key == K_LEFT and not self.turnP.isRunning():
self.turnP.ang = 0.5
self.turnP.start()
return progress("(say) Turn left")
if evt.key == K_RIGHT and not self.turnP.isRunning():
self.turnP.ang = -0.5
self.turnP.start()
return progress("(say) Turn right")
### DO NOT MODIFY -----------------------------------------------
else: # Use superclass to show any other events
return JoyApp.onEvent(self, evt)
return # ignoring non-KEYDOWN events
class MainApp(JoyApp):
""" Main app to run the program that controls our robot """
def __init__(self, wphAddr, *arg, **kw):
cfg = dict()
JoyApp.__init__(self, cfg=cfg, *arg, **kw)
self.srvAddr = (wphAddr, APRIL_DATA_PORT)
def onStart(self):
self.xMotors = [self.robot.at.Nx04, self.robot.at.Nx02]
self.yMotors = [self.robot.at.Nx06, self.robot.at.Nx08]
self.motorPlan = MotorPlan(self)
self.core = Core(Mode.ACTUAL, self)
self.core.start()
self.timeForFilter = self.onceEvery(1.0 / 10.0)
self.startedFilter = False
self.auto = False
self.timeForServoMeasure = self.onceEvery(1.0 / 2.0)
#sensor stuff
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
# Setup autonomous mode timer
self.timeForAuto = self.onceEvery(10)
def onEvent(self, evt):
if self.timeForFilter():
self.core.setSensorAndWaypoints(
array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]),
self.sensor.lastWaypoints[1])
try:
print("Estimated Pos: " +
str(self.core.particleFilter.getState().pos) + "\t" +
str(self.core.particleFilter.getState().yaw))
print("Waypoints: " + str(self.sensor.lastWaypoints[1]))
except:
pass
if self.timeForAuto() and self.auto:
self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1])
if self.timeForServoMeasure():
pass
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
class RobotSimulatorApp(JoyApp):
"""Concrete class RobotSimulatorApp <<singleton>>
A JoyApp which runs the DummyRobotSim 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", *arg, **kw)
self.srvAddr = (wphAddr, APRIL_DATA_PORT)
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.robSim = DummyRobotSim(fn=None)
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1 / 15.0)
self.timeForFrame = self.onceEvery(1 / 20.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
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()
progress(self.robSim.logLaserValue(self.now))
# 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 evt.type == KEYDOWN:
if evt.key == K_UP:
self.robSim.move(0.5)
return progress("(say) Move forward")
elif evt.key == K_DOWN:
self.robSim.move(-0.5)
return progress("(say) Move back")
elif evt.key == K_LEFT:
self.robSim.turn(0.5)
return progress("(say) Turn left")
elif evt.key == K_RIGHT:
self.robSim.turn(-0.5)
return progress("(say) Turn right")
# Use superclass to show any other events
else:
return JoyApp.onEvent(self, evt)
return # ignoring non-KEYDOWN events
class RedBuggyApp(JoyApp):
def __init__(self, wphAddr=WAYPOINT_HOST, *arg, **kw):
JoyApp.__init__(self, confPath="$/cfg/JoyApp.yml", *arg, **kw)
self.srvAddr = (wphAddr, APRIL_DATA_PORT)
def onStart(self):
# Init sensor plan
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
# Load plans here
self.moveP = MovePlan(self)
self.turnP = RotatePlan(self)
self.turretP = TurretPlan(self)
self.autoP = AutoPlan(self, self.sensor, self.moveP, self.turnP)
def onEvent(self, evt):
if evt.type != KEYDOWN:
return
if evt.type == KEYDOWN:
ts, f, b = self.sensor.lastSensor
ts_w, w = self.sensor.lastWaypoints
progress("(say)f: " + str(f))
progress("(say)b: " + str(b))
progress("(say)w: " + str(w))
if evt.key == K_UP and not self.moveP.isRunning():
# Forward plan
self.moveP.torque = MOVE_TORQUE
self.moveP.start()
return progress("(say) Move forward")
elif evt.key == K_DOWN and not self.moveP.isRunning():
# Backward plan
self.moveP.torque = -1 * MOVE_TORQUE
self.moveP.start()
return progress("(say) Move back")
elif evt.key == K_LEFT and not self.turnP.isRunning():
# Turn left plan
self.turnP.torque = -1 * TURN_TORQUE
self.turnP.start()
return progress("(say) Turn left")
elif evt.key == K_RIGHT and not self.turnP.isRunning():
# Turn right plan
self.turnP.torque = TURN_TORQUE
self.turnP.start()
return progress("(say) Turn right")
elif evt.key == K_a and not (self.turnP.isRunning()
or self.moveP.isRunning()):
self.autoP.start()
return progress("(say) Moving autonomously")
elif evt.key == K_s and not (self.turnP.isRunning()
or self.moveP.isRunning()):
self.autoP.stop()
return progress("(say) Stop autonomous control")
elif evt.key == K_m and not (self.turnP.isRunning()
or self.moveP.isRunning()):
self.turretP.turretSpeed = TURRET_TORQUE
self.turretP.start()
return progress("(say) adjusted turret")
elif evt.key == K_n and not (self.turnP.isRunning()
or self.moveP.isRunning()):
self.turretP.turretSpeed = -1 * TURRET_TORQUE
self.turretP.start()
return progress("(say) adjusted turret")
class MainApp(JoyApp):
""" Main app to run the program that controls our robot """
def __init__(self, wphAddr, *arg, **kw):
cfg = dict()
JoyApp.__init__(self, cfg=cfg, *arg, **kw)
self.srvAddr = (wphAddr, APRIL_DATA_PORT)
def onStart(self):
self.xMotors = [self.robot.at.Nx04, self.robot.at.Nx02]
self.yMotors = [self.robot.at.Nx06, self.robot.at.Nx08]
self.motorPlan = MotorPlan(self)
self.core = Core(Mode.ACTUAL, self)
self.core.start()
self.timeForFilter = self.onceEvery(1.0/10.0)
self.startedFilter = False
self.auto = False
self.timeForServoMeasure = self.onceEvery(1.0/2.0)
#sensor stuff
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
# Setup autonomous mode timer
self.timeForAuto = self.onceEvery(10)
def onEvent(self, evt):
if self.timeForFilter():
self.core.setSensorAndWaypoints(array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]),
self.sensor.lastWaypoints[1])
try:
print("Estimated Pos: " + str(self.core.particleFilter.getState().pos) + "\t" + str(self.core.particleFilter.getState().yaw))
print("Waypoints: " + str(self.sensor.lastWaypoints[1]))
except:
pass
if self.timeForAuto() and self.auto:
self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1])
if self.timeForServoMeasure():
pass
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
