def goMapping(freq=30):
global myMap,q
# Parameters
RED_BALL = 4
GREEN_BALL = 5
myMap.initMapping()
while not q:
start = time.time()
sensorPoints = ct.getSensorPoints()
pose = ct.getPose()
balls = [x for x in v.getAreas(pose) if v.isBall(x)]
knownBalls = []
for b in balls:
bc = v.getBallCoords(b,pose)
if bc is not None:
knownBalls.append(bc)
# Mapping update
myMap.robotPositioned(ctypes.c_double(pose[0]), ctypes.c_double(pose[1]))
for a in knownBalls:
myMap.ballDetected(ctypes.c_double(a[0]),ctypes.c_double(a[1]),ctypes.c_int(RED_BALL))
for s in sensorPoints:
if(s[2]):
myMap.wallDetected(ctypes.c_double(s[0]), ctypes.c_double(s[1]))
else:
myMap.wallNotDetected(ctypes.c_double(s[0]), ctypes.c_double(s[1]))
time.sleep(max(0,1.0/float(freq) - (time.time()-start)))
def goPygame():
global myMap,q,recordedBalls
pygame.init()
# Parameters
width,height = 900,900
realWidth,realHeight = 450,450
ROBOT_RADIUS = 7
BALL_RADIUS = 2.5/2.0
RED_COLOR = pygame.Color(255,0,0)
GREEN_COLOR = pygame.Color(0,255,0)
WHITE_COLOR = pygame.Color(255,255,255)
BLACK_COLOR = pygame.Color(0,0,0)
BLUE_COLOR = pygame.Color(0,0,255)
prop = int((width/realWidth + height/realHeight)/2)
fpsClock = pygame.time.Clock()
pygame.display.set_caption('Robot location and vision')
main_surface = pygame.display.set_mode((width, height))
fieldMap = pygame.transform.scale(
pygame.image.load("simulator/map.bmp"),
(width,height))
main_surface.fill(WHITE_COLOR)
#main_surface.blit(fieldMap, (0,0))
while not q:
pose = ct.getPose()
sensorPoints = ct.getSensorPoints()
#main_surface.blit(fieldMap, (0,0))
main_surface.fill(WHITE_COLOR)
#balls = v.getAreas(pose)
#for b in balls:
# pygame.draw.circle(main_surface, BLUE_COLOR,
# (int(b[0])*prop, (realHeight-1-int(b[1]))*prop),
# int(BALL_RADIUS)*prop, 0)
pygame.draw.circle(main_surface, BLUE_COLOR,
(int(pose[0])*prop,(realHeight-1-int(pose[1]))*prop),
ROBOT_RADIUS*prop, prop)
for s in sensorPoints:
if(s[2]):
color = RED_COLOR
else:
color = BLACK_COLOR
pygame.draw.line(main_surface, color,
(int(pose[0])*prop,(realHeight-1-int(pose[1]))*prop),
(int(s[0])*prop,(realHeight-1-int(s[1]))*prop),
prop)
for x in xrange(realWidth):
for y in xrange(realHeight):
if(myMap.getWall(x,y)):
pygame.draw.rect(main_surface, RED_COLOR,(x*prop,(realHeight-1-y)*prop,prop,prop))
elif(myMap.getBall(x,y)):
pygame.draw.circle(main_surface, BLUE_COLOR,
(x*prop, (realHeight-1-y)*prop),
int(BALL_RADIUS)*prop, 0)
for event in pygame.event.get():
if event.type == QUIT:
pygame.quit()
q = True
if not q:
pygame.display.update()
fpsClock.tick(50)
