def step():
global inp
robot.count += 1
inp = io.SensorInput(cheat=True)
# discretize sonar readings
# each element in discreteSonars is a tuple (d, cells)
# d is the distance measured by the sonar
# cells is a list of grid cells (r,c) between the sonar and the point d meters away
discreteSonars = []
for (sonarPose, distance) in zip(sonarDist.sonarPoses,inp.sonars):
if NOISE_ON:
distance = noiseModel.noisify(distance, gridSquareSize)
sensorIndices = robot.map.pointToIndices(inp.odometry.transformPose(sonarPose).point())
hitIndices = robot.map.pointToIndices(sonarDist.sonarHit(distance, sonarPose, inp.odometry))
ray = util.lineIndices(sensorIndices, hitIndices)
discreteSonars.append((distance, ray))
# figure out where robot is
startPoint = inp.odometry.point()
startCell = robot.map.pointToIndices(startPoint)
# if necessary, make new plan
if robot.plan is None:
print 'REPLANNING'
robot.plan = search.ucSearch(robot.successors,
robot.map.pointToIndices(inp.odometry.point()),
lambda x: x == robot.goalIndices,
lambda x: 0)
isGood = True
for i in robot.plan:
if not robot.map.isPassable(i) and isGood:
print 'REPLANNING'
robot.plan = search.ucSearch(robot.successors,
robot.map.pointToIndices(inp.odometry.point()),
lambda x: x == robot.goalIndices,
lambda x: 0)
isGood = False
# graphics (draw robot's plan, robot's location, goalPoint)
# do not change this block
for w in robot.windows:
w.redrawWorld()
robot.windows[-1].markCells(robot.plan,'blue')
robot.windows[-1].markCell(robot.map.pointToIndices(inp.odometry.point()),'gold')
robot.windows[-1].markCell(robot.map.pointToIndices(goalPoint),'green')
# update map
for (d,cells) in discreteSonars:
if d != 5.0:
robot.map.sonarHit(cells[-1])
# if we are within 0.1m of the goal point, we are done!
if startPoint.distance(goalPoint) <= 0.1:
io.Action(fvel=0,rvel=0).execute()
code = checkoff.generate_code(globals())
raise Exception('Goal Reached!\n\n%s' % code)
# otherwise, figure out where to go, and drive there
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
while startPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
robot.plan.pop(0)
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
currentAngle = inp.odometry.theta
angleDiff = startPoint.angleTo(destinationPoint)-currentAngle
angleDiff = util.fixAnglePlusMinusPi(angleDiff)
fv = rv = 0
if startPoint.distance(destinationPoint) > 0.1:
if abs(angleDiff) > 0.1:
rv = angleDiff
else:
fv = 1.5*startPoint.distance(destinationPoint)
io.setForward(fv)
io.setRotational(rv)
# render the drawing windows
# do not change this block
for w in robot.windows:
w.render()
def step():
robot.count += 1
inp = io.SensorInput(cheat=True)
for c in ('orange','cyan','blue','red'):
robot.map.clearColor(c)
# discretize sonar readings
# each element in discreteSonars is a tuple (d, cells)
# d is the distance measured by the sonar
# cells is a list of grid cells (r,c) between the sonar and the point d meters away
discreteSonars = []
for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
if NOISE_ON:
r = random.random()
if d != 5:
if r > .99:
d = 5
elif r > .97:
d = random.uniform(gridSquareSize,d)
else:
if r > .98:
d = random.uniform(gridSquareSize,1.5)
discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))
# update map
for (d,cells) in discreteSonars:
if d != 5:
robot.map.set(cells[-1])
for index in range(0, len(cells)-1):
robot.map.clear(cells[index])
# if necessary, make new plan
if robot.plan is None or not all(robot.map.isPassable(i) for i in robot.plan):
print 'REPLANNING'
robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
robot.map.pointToIndices(inp.odometry.point()),None,0),
lambda x: x == robot.map.pointToIndices(goalPoint),
lambda x: 0)
# graphics (draw robot's plan, robot's location, goalPoint)
robot.map.markCells(robot.plan,'blue')
robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'red')
robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')
# move to target point (similar to driving task in DL2)
currentPoint = inp.odometry.point()
currentAngle = inp.odometry.theta
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
robot.plan.pop(0)
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
if not currentPoint.isNear(destinationPoint,0.1):
angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
if abs(angle)<0.1:
#if close enough to pointing, use proportional control on forward velocity
fv = 2*currentPoint.distance(destinationPoint)
rv = 0
else:
#otherwise, use proportional control on angular velocity
fv = 0
rv = 2*angle
else:
raise Exception, 'Goal Reached!'
robot.map.update()
io.Action(fvel=fv,rvel=rv).execute()
def step():
robot.count += 1
inp = io.SensorInput(cheat=True)
for c in ('orange','cyan','blue','red'):
robot.map.clearColor(c)
# discretize sonar readings
# each element in discreteSonars is a tuple (d, cells)
# d is the distance measured by the sonar
# cells is a list of grid cells (r,c) between the sonar and the point d meters away
discreteSonars = []
for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
if NOISE_ON:
d = noiseModel.noisify(d,gridSquareSize)
discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))
# update map
for (d,cells) in discreteSonars:
# update probabilities
if(d<=1.5):
robot.mentalmap[cells[-1]].update(True)
for c in cells[:-1]:
robot.mentalmap[c].update(False)
else:
for i in range(0, len(cells)/4):
robot.mentalmap[cells[i]].update(False)
# update the map
for c in cells:
belief = robot.mentalmap[c].belief
state = belief.maxProbElt()
if(state):
robot.map.sonarHit(c)
else:
robot.map.sonarPass(c)
# null the path if necessary
if not robot.plan is None:
for pt in robot.plan:
if(not robot.map.isPassable(pt)):
robot.plan = None
break
if robot.plan is None:
print 'REPLANNING'
robot.dirty = True
robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
robot.map.pointToIndices(inp.odometry.point()),None,0),
lambda x: x == robot.map.pointToIndices(goalPoint),
lambda x: 0)
# graphics (draw robot's plan, robot's location, goalPoint)
# do not change this block
robot.map.markCells(robot.plan,'blue')
robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'red')
robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')
# move to target point (similar to driving task in DL2)
currentPoint = inp.odometry.point()
currentAngle = inp.odometry.theta
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
# check if we are ahead in the path
if(robot.dirty):
for i in range(0, len(robot.plan)):
if(robot.map.pointToIndices(currentPoint)==robot.plan[i]):
destination = robot.map.indicesToPoint(robot.plan[i+1])
while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
robot.plan.pop(0)
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
if not currentPoint.isNear(destinationPoint,0.1):
angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
if abs(angle)<0.1:
#if close enough to pointing, use proportional control on forward velocity
fv = SPEED*currentPoint.distance(destinationPoint)
rv = 0
else:
#otherwise, use proportional control on angular velocity
fv = 0
rv = ANG_SPEED*angle
else:
t = time.time() - robot.startTime;o=inp.odometry.xytTuple();w=THE_WORLD[:1]+[THE_WORLD[1].xyTuple()]+THE_WORLD[2:];n=NOISE_ON
raise Exception, 'Goal Reached!\n\n%s' % e(repr((t,o,w,n)))
robot.map.update()
io.Action(fvel=fv,rvel=rv).execute()
robot.dirty = True
def step():
global inp
robot.count += 1
inp = io.SensorInput(cheat=True)
for c in ('orange','cyan','blue','red','gold'):
robot.map.clearColor(c)
if robot.map.doHeatMap:
robot.map.heatMap()
# discretize sonar readings
# each element in discreteSonars is a tuple (d, cells)
# d is the distance measured by the sonar
# cells is a list of grid cells (r,c) between the sonar and the point d meters away
discreteSonars = []
for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
if NOISE_ON:
d = noiseModel.noisify(d,gridSquareSize)
if d < 1.5:
discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))
# update map
for (d,cells) in discreteSonars:
robot.map.sonarHit(cells[-1])
# if necessary, make new plan
secondary_sonars = discreteSonars[:3] + discreteSonars[5:]
if robot.plan is None or tooCloseToWall(discreteSonars[3:5], secondary_sonars):
print 'REPLANNING'
robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
robot.map.pointToIndices(inp.odometry.point()),None,0),
lambda x: x == robot.map.pointToIndices(goalPoint),
lambda x: 0)
# graphics (draw robot's plan, robot's location, goalPoint)
# do not change this block
if robot.map.showPassable:
robot.map.markNotPassable()
if robot.plan is not None:
robot.map.markCells(robot.plan,'blue')
robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'gold')
robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')
# move to target point (similar to driving task in DL2)
currentPoint = inp.odometry.point()
currentAngle = inp.odometry.theta
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
robot.plan.pop(0)
destinationPoint = robot.map.indicesToPoint(robot.plan[0])
if not currentPoint.isNear(destinationPoint,0.1):
angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
if abs(angle)<0.1:
#if close enough to pointing, use proportional control on forward velocity
fv = 2*currentPoint.distance(destinationPoint)
rv = 0
else:
#otherwise, use proportional control on angular velocity
fv = 0
rv = 2*angle
else:
raise Exception,'Goal Reached!\n\n%s' % checkoff.generate_code(globals())
robot.map.render()
io.Action(fvel=fv,rvel=rv).execute()