def __init__(self, cols, rows, widthMM, heightMM,
#the x,y and th of the current location can be passed
currentRow = -1, currentCol = -1, currentTh= 0,
goalRow = -1, goalCol = -1, gridUpdate = "",
image=None, grid=None, gridResize=1.0,showIter=0, title=""):
self.markerToolkit = MarkerToolkit()
self.x_localizeMM = 0
self.y_localizeMM = 0
self.thr_localize = 0
self.x_last_marker_localizeMM = 0
self.x_last_marker_robotMM = 0
self.y_last_marker_localizeMM = 0
self.y_last_marker_robotMM = 0
self.thr_last_marker_localize = 0
self.thr_last_marker_robot = 0
self.resetMarkers()
OccupancyGrid.__init__(self, cols=cols, rows=rows,
widthMM=widthMM, heightMM=heightMM,
currentRow=currentRow, currentCol=currentCol,
currentTh=currentTh,
goalRow=goalRow, goalCol=goalCol,
image=image, grid=grid, gridResize=gridResize,
showIter=showIter, title=title)
def __init__(
self,
cols,
rows,
widthMM,
heightMM,
# the x,y and th of the current location can be passed
currentRow=-1,
currentCol=-1,
currentTh=0,
goalRow=-1,
goalCol=-1,
gridUpdate="",
image=None,
grid=None,
gridResize=1.0,
showIter=0,
title="",
):
self.centros = []
self.markerToolkit = MarkerToolkit()
self.x_localizeMM = 0
self.y_localizeMM = 0
self.thr_localize = 0
self.x_last_marker_localizeMM = 0
self.x_last_marker_robotMM = 0
self.y_last_marker_localizeMM = 0
self.y_last_marker_robotMM = 0
self.thr_last_marker_localize = 0
self.thr_last_marker_robot = 0
self.resetMarkers()
OccupancyGrid.__init__(
self,
cols=cols,
rows=rows,
widthMM=widthMM,
heightMM=heightMM,
currentRow=currentRow,
currentCol=currentCol,
currentTh=currentTh,
goalRow=goalRow,
goalCol=goalCol,
image=image,
grid=grid,
gridResize=gridResize,
showIter=showIter,
title=title,
)
self.distanceToWall = [[self.infinity for col in range(self.cols)] for row in range(self.rows)]
# Todos los bloques ocupados forman parte de una pared y por tanto
# se les asigna distancia 0 a las paredes.
for row in range(self.rows):
for col in range(self.cols):
if self.grid[row][col] > self.threshhold:
self.distanceToWall[row][col] = 0
# Inicia perceptrón con los pesos calculados en la parte1. Si no se desea usar, comentar
# esta sección de código.
self.pesos = [
-0.25830320437434307,
1.160526501403915,
-0.15551005292679421,
1.2578377038143795,
-0.55707372041035363,
-2.9552247878192888,
-1.6809527155003046,
-2.2014578204055359,
-13.0,
0.0,
]
self.calculadorNeuronal = PerceptronMonocapa(self.pesos, w2)
class Navigator(OccupancyGrid):
NO_FORWARD = 0
SLOW_FORWARD = 0.05
MED_FORWARD = 0.3 # 0.5
FULL_FORWARD = 0.5 # 1.0
NO_TURN = 0
MED_LEFT = 0.5
HARD_LEFT = 1.0
MED_RIGHT = -0.5
HARD_RIGHT = -1.0
def __init__(
self,
cols,
rows,
widthMM,
heightMM,
# the x,y and th of the current location can be passed
currentRow=-1,
currentCol=-1,
currentTh=0,
goalRow=-1,
goalCol=-1,
gridUpdate="",
image=None,
grid=None,
gridResize=1.0,
showIter=0,
title="",
):
self.centros = []
self.markerToolkit = MarkerToolkit()
self.x_localizeMM = 0
self.y_localizeMM = 0
self.thr_localize = 0
self.x_last_marker_localizeMM = 0
self.x_last_marker_robotMM = 0
self.y_last_marker_localizeMM = 0
self.y_last_marker_robotMM = 0
self.thr_last_marker_localize = 0
self.thr_last_marker_robot = 0
self.resetMarkers()
OccupancyGrid.__init__(
self,
cols=cols,
rows=rows,
widthMM=widthMM,
heightMM=heightMM,
currentRow=currentRow,
currentCol=currentCol,
currentTh=currentTh,
goalRow=goalRow,
goalCol=goalCol,
image=image,
grid=grid,
gridResize=gridResize,
showIter=showIter,
title=title,
)
self.distanceToWall = [[self.infinity for col in range(self.cols)] for row in range(self.rows)]
# Todos los bloques ocupados forman parte de una pared y por tanto
# se les asigna distancia 0 a las paredes.
for row in range(self.rows):
for col in range(self.cols):
if self.grid[row][col] > self.threshhold:
self.distanceToWall[row][col] = 0
# Inicia perceptrón con los pesos calculados en la parte1. Si no se desea usar, comentar
# esta sección de código.
self.pesos = [
-0.25830320437434307,
1.160526501403915,
-0.15551005292679421,
1.2578377038143795,
-0.55707372041035363,
-2.9552247878192888,
-1.6809527155003046,
-2.2014578204055359,
-13.0,
0.0,
]
self.calculadorNeuronal = PerceptronMonocapa(self.pesos, w2)
def findPath(self, event=None):
print ("FIND PATH:", self.getGoalRow(), self.getGoalCol())
if self.getCurrentRow() < 0 or self.getCurrentCol() < 0:
raise Exception("NoPathExists", "current location unknown")
if self.getGoalCol() < 0 or self.getGoalRow() < 0:
raise Exception("NoPathExists", "goal location unknown")
if self.grid[self.getGoalRow()][self.getGoalCol()] > self.threshhold:
raise Exception("NoPathExists", "goal is in unattainable location")
if self.debug:
print "Finding path..."
self.calculateAllCosts(goalCol=self.getGoalCol(), goalRow=self.getGoalRow(), maxIters=self.rows * self.cols)
(pathGrid, pathList) = self.getPath(row=self.getCurrentRow(), col=self.getCurrentCol())
# self.getPath(row=self.getCurrentRow(),
# col=self.getCurrentCol())
# return
if pathGrid:
print "Done!"
self.redraw(self.distanceToWall, path=pathGrid)
# self.redraw(self.segmentacion,pathGrid,stepByStep)
else:
raise Exception("NoPathExists", "maximum interation limit exceded")
return pathList
def distanciaManhattan(self, i, j, l, m):
return abs(i - l) + abs(j - m)
def distanciaEuclidea(self, i, j, l, m):
return math.sqrt((i - l) ** 2 + (i - m) ** 2)
def costeF(self, G, i, j):
# H = self.distanciaManhattan(self.getGoalRow(),self.getGoalCol(),i,j)
H = max(
self.distanciaEuclidea(self.getGoalRow(), self.getGoalCol(), i, j),
self.distanciaManhattan(self.getGoalRow(), self.getGoalCol(), i, j),
)
return G + H
def costesFGH(self, n, G):
return (
self.costeF(G, self.centros[n][0], self.centros[n][1]),
G,
self.distanciaManhattan(self.getGoalRow(), self.getGoalCol(), self.centros[n][0], self.centros[n][1]),
)
def aEstrella(self, MA, n):
# print n, self.nodoObjetivo
if n == self.nodoObjetivo:
return [n]
nnodos = len(self.centros)
listaAbierta = set([n])
almacenFGH = [(0, 0, 0) for i in range(nnodos)]
almacenFGH[n] = self.costesFGH(n, 0)
listaCerrada = set()
padres = [0 for i in range(nnodos)]
camino = []
g = 0
alcanzado = False
while not alcanzado and len(listaAbierta) > 0:
candidato = -1
iterok = False
minimo = self.infinity
for k in range(nnodos):
if (
self.costeF(g, self.centros[k][0], self.centros[k][1]) < minimo
and k in listaAbierta
and self.grid[self.centros[k][0]][self.centros[k][1]] < self.threshhold
):
minimo = self.costeF(g, self.centros[k][0], self.centros[k][1])
candidato = k
camino = camino + [candidato]
g = almacenFGH[candidato][2]
listaAbierta.remove(candidato)
listaCerrada.add(candidato)
for k in range(nnodos):
if candidato <> k and (MA[candidato][k] > 0 or MA[k][candidato] > 0): # Si es nodo adyacente
if (
not k in listaCerrada and self.grid[self.centros[k][0]][self.centros[k][1]] < self.threshhold
): # Si el adyacente no está en la lista cerrada ni es pared
iterok = True
if not k in listaAbierta: # Si no estaba en la lista abierta.
listaAbierta.add(k)
if k == self.nodoObjetivo:
alcanzado = True
padres[k] = candidato
almacenFGH[k] = self.costesFGH(k, g + 1)
else: # Si ya estaba en la lista abierta.
if g + 1 < almacenFGH[k][2]: # Si es mejor el camino actual que el por el que se llegó a k
padre[k] = candidato
almacenFGH[k] = self.costesFGH(k, self.centros[k][1])
if not iterok:
return []
if alcanzado:
return camino + [self.nodoObjetivo]
else:
return []
def dijkstra(self, MA, n):
# MA = Matriz de adyacencia del grafo.
# n = nodo origen
nnodos = len(self.centros)
retorno = [[0 for j in range(nnodos)] for i in range(nnodos)]
L = [self.infinity for i in range(nnodos)]
L[n] = 0
S = set(range(nnodos))
while len(S) > 1:
minimo = self.infinity
candidato = -1
for p in range(nnodos):
if p in S and L[p] < minimo:
minimo = L[p]
candidato = p
for q in range(nnodos):
if q in S:
if MA[candidato][q] > 0 or MA[q][candidato] > 0:
peso = 1
else:
peso = self.infinity
if (
min(L[q], L[candidato] + peso) == L[candidato] + peso
and min(L[q], L[candidato] + peso) < self.infinity
):
retorno[candidato][q] = peso
L[q] = min(L[q], L[candidato] + peso)
S.remove(candidato)
return retorno
def getPath(self, row, col):
pathGrid = [[0 for c in range(self.cols)] for r in range(self.rows)]
pathList = []
# El primer paso para construir el grafo de adyacencia entre habitaciones
# para ello, se consideran adyacentes aquellas habitaciones que tienen
# un punto adyacente con otro de otra habitación (dos puntos
# adyacentes que en la matriz "segmentacion" tienen etiquetas diferentes.
nnodos = len(self.centros) # Número de nodos (habitaciones)
MA = [[0 for j in range(nnodos)] for i in range(nnodos)] # Inicializa matriz de adyacencia
MAF = [[(0, 0) for j in range(nnodos)] for i in range(nnodos)] # Puntos frontera en cada adyacencia
for i in range(self.rows):
for j in range(self.cols):
if self.grid[i][j] < self.threshhold:
eti = self.segmentacion[i][j]
if i > 0 and i < self.rows - 1 and j > 0 and j < self.cols - 1:
for l in range(-1, 2):
for m in range(-1, 2):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i > 0 and i < self.rows - 1 and j == self.cols - 1:
for l in range(-1, 2):
for m in range(-1, 1):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i > 0 and i < self.rows - 1 and j == 0:
for l in range(-1, 2):
for m in range(0, 2):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i == self.rows - 1 and j > 0 and j < self.cols - 1:
for l in range(-1, 1):
for m in range(-1, 2):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i == 0 and j > 0 and j < self.cols - 1:
for l in range(0, 2):
for m in range(-1, 2):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i == 0 and j == 0:
for l in range(0, 2):
for m in range(0, 1):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i == 0 and j == self.cols - 1:
for l in range(0, 2):
for m in range(-1, 1):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i == self.rows - 1 and j == 0:
for l in range(-1, 1):
for m in range(0, 2):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
elif i == self.rows - 1 and j == self.cols - 1:
for l in range(-1, 1):
for m in range(-1, 1):
if self.segmentacion[i + l][j + m] <> eti and self.grid[i + l][j + m] < self.threshhold:
MA[eti][self.segmentacion[i + l][j + m]] = 1
MAF[eti][self.segmentacion[i + l][j + m]] = (i, j)
# Una vez obtenido el grafo (su matriz de adyacencia), se busca el camino topológico
# más corto desde el origen hasta la habitación objetivo, para ello, se ha utilizado
# el algoritmo A*
caminoCentros = self.aEstrella(MA, self.segmentacion[row][col])
if len(caminoCentros) == 0:
return [[], []]
for k in range(1, len(caminoCentros)):
frontera = MAF[caminoCentros[k - 1]][caminoCentros[k]]
i = self.centros[caminoCentros[k]][0]
j = self.centros[caminoCentros[k]][1]
pathGrid[i][j] = 1
pathGrid[frontera[0]][frontera[1]] = 1
pathList.append(frontera)
pathList.append((i, j))
return (pathGrid, pathList)
def clasificaPuntosSegmentacion(self, pi, pj):
i = pi
j = pj
encontrado = False
while not encontrado and i < self.rows and j < self.cols:
ci = i
cj = j
if i > 0 and i < self.rows - 1 and j > 0 and j < self.cols - 1:
maximo = 0
for l in range(-1, 2):
for m in range(-1, 2):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i > 0 and i < self.rows - 1 and j == self.cols - 1:
maximo = 0
for l in range(-1, 2):
for m in range(-1, 1):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i > 0 and i < self.rows - 1 and j == 0:
maximo = 0
for l in range(-1, 2):
for m in range(0, 2):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i == self.rows - 1 and j > 0 and j < self.cols - 1:
maximo = 0
for l in range(-1, 1):
for m in range(-1, 2):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i == 0 and j > 0 and j < self.cols - 1:
maximo = 0
for l in range(0, 2):
for m in range(-1, 2):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i == 0 and j == 0:
maximo = 0
for l in range(0, 2):
for m in range(0, 1):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i == 0 and j == self.cols - 1:
maximo = 0
for l in range(0, 2):
for m in range(-1, 1):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i == self.rows - 1 and j == 0:
maximo = 0
for l in range(-1, 1):
for m in range(0, 2):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
elif i == self.rows - 1 and j == self.cols - 1:
maximo = 0
for l in range(-1, 1):
for m in range(-1, 1):
if self.distanceToWall[i + l][j + m] > maximo and self.grid[i + l][j + m] < self.threshhold:
maximo = self.distanceToWall[i + l][j + m]
ci = i + l
cj = j + m
if ci == i and cj == j:
encontrado = True
i = ci
j = cj
if not [i, j] in self.centros:
self.centros = self.centros + [[i, j]]
return (i, j, maximo)
def calculateAllCosts(self, goalCol, goalRow, maxIters):
"""
Versión simplificada del algoritmo "watershed".
When an occupancy probability is above some threshold, assume
that the cell is occupied.
"""
# if (goalCol == self.lastGoalDistCol and
# goalRow == self.lastGoalDistRow):
# return
# startTime = time.time()
print ("Calculando todas las distancias a paredes")
self.distanceToWall = [[self.infinity for col in range(self.cols)] for row in range(self.rows)]
self.segmentacion = [[0 for j in range(self.cols)] for i in range(self.rows)]
# Todos los bloques ocupados forman parte de una pared y por tanto
# se les asigna distancia 0 a las paredes.
for row in range(self.rows):
for col in range(self.cols):
if self.grid[row][col] > self.threshhold:
self.distanceToWall[row][col] = 0
if not self.inRange(goalRow, goalCol):
raise Exception("goalOutOfMapRange")
for iter in range(maxIters):
valuesChanged = 0
if self.showIterations:
print "Mostrando resultado tras la iteración:> ", iter
self.redraw(self.distanceToWall, stepByStep=1)
self.frame.update()
# time.sleep(10)
# print "ITERACION: ", iter
for row in range(self.rows):
for col in range(self.cols):
for i in [-1, 0, 1]:
for j in [-1, 0, 1]:
if self.inRange(row + i, col + j):
if self.grid[row][col] > self.threshhold:
self.distanceToWall[row][col] = 0
else:
if abs(i) == 0 and abs(j) == 0:
d = 0.00
elif abs(i) == 1 and abs(j) == 1:
if self.tooTight(row, col, i, j):
d = self.infinity
else:
d = 1.41
# d = 1.41
else:
d = 1.00
# adj = self.distanceToWall[row+i][col+j] + self.grid[row+i][col+j] + d
adj = self.distanceToWall[row + i][col + j] + d
# print adj<self.distanceToWall[row][col]
# print "ADJ: ", adj, "ANT: ", self.distanceToWalll[row][col]
# if self.distanceToWalll[row][col] != 0:
# print "IT: ",adj < self.distanceToWall[row][col]
if adj < self.distanceToWall[row][col]:
valuesChanged += 1
self.distanceToWall[row][col] = min(self.distanceToWall[row][col], adj)
if valuesChanged == 0:
# endTime = time.time()
print "Distancias a paredes calculadas en %d iterationes." % iter
# print "Tiempo transcurrido %0.3f ms." % ((endTime-startTime)*1000.0)
break
self.centros = [] # Puntos representantes de cada habitacion encontrada.
buff = []
contador = 0.1
anterior = self.segmentacion[i][j] = self.clasificaPuntosSegmentacion(0, 0)[2]
for i in range(self.rows):
for j in range(self.cols):
if self.grid[i][j] < self.threshhold:
buff = buff + [(i, j, self.clasificaPuntosSegmentacion(i, j))]
# self.segmentacion[buff[0]][buff[1]] = 1
"""if self.grid[i][j] < self.threshhold:
buff = self.clasificaPuntosSegmentacion(i,j)[2]
self.segmentacion[i][j] = self.clasificaPuntosSegmentacion(i,j)[2]
if buff != anterior:
contador = contador + 0.1
anterior = buff"""
self.numeracionCentros = [[0 for j in range(self.cols)] for i in range(self.rows)]
for i in range(len(self.centros)):
self.numeracionCentros[self.centros[i][0]][self.centros[i][1]] = i
for i in range(len(buff)):
self.segmentacion[buff[i][0]][buff[i][1]] = self.numeracionCentros[buff[i][2][0]][buff[i][2][1]]
if buff[i][0] == self.getGoalRow() and buff[i][1] == self.getGoalCol():
self.nodoObjetivo = self.numeracionCentros[buff[i][2][0]][buff[i][2][1]]
SegmentacionPintada = [
[float(self.segmentacion[i][j]) / float(len(self.segmentacion) - 1) * 1.5 for j in range(self.cols)]
for i in range(self.rows)
]
for i in range(len(self.centros)):
SegmentacionPintada[self.centros[i][0]][self.centros[i][1]] = 0
self.redraw(SegmentacionPintada, stepByStep=1)
self.frame.update()
# time.sleep(10)
def resetMarkers(self):
self.markers = {}
self.markerLoc = {}
self.markerAngle = {}
# we use the same angle convention used in the pyrobot simulator
# angles are expressed in radians, 0 is north and + angles rotate
# counter-clockwise
def placeMarker(self, row, col, name, angle=0, redraw=0):
self.markers[name] = (row, col)
self.markerLoc[(row, col)] = name
self.markerAngle[name] = angle
if redraw:
self.redrawTrig.set(1)
def placeRelativeMarker(self, name, relXOffM, relYOffM, relTh, redraw=1):
# the -1's are to change the perspective.
newMarkerInfo = self.getPosRelativeToKnown(
self.getCurrentRow() * self.rowScaleMM,
self.getCurrentCol() * self.colScaleMM,
self.getCurrentTh(),
relXOffM,
relYOffM * -1,
relTh * -1,
)
print ("placing new marker ", name, newMarkerInfo[0], newMarkerInfo[1], newMarkerInfo[4])
self.placeMarker(row=newMarkerInfo[0], col=newMarkerInfo[1], name=name, angle=newMarkerInfo[4], redraw=1)
def getPosRelativeToKnown(self, knownRowMM, knownColMM, knownAngle, relXOffM, relYOffM, relTh):
# print "relXOffM",relXOffM,"relYOffM",relYOffM,"relTh",relTh,\
# "atan",math.atan(relYOffM/relXOffM),"knownAngle",knownAngle
toKnownDistMM = calculateHypotenuse(relXOffM * 1000, relYOffM * 1000)
toKnownAngle = knownAngle + relTh
# print "toKnownAngle",toKnownAngle,"toKnownDistMM",toKnownDistMM,\
# "cos-row",math.cos(toKnownAngle),"sin-col",math.sin(toKnownAngle)
# print "rowMMDiff ", (toKnownDistMM * math.cos(toKnownAngle)), \
# "colMMDiff ", (toKnownDistMM * math.sin(toKnownAngle))
relRowMM = knownRowMM - (toKnownDistMM * math.cos(toKnownAngle))
relColMM = knownColMM - (toKnownDistMM * math.sin(toKnownAngle))
relRow = int(round(relRowMM / self.rowScaleMM))
relCol = int(round(relColMM / self.colScaleMM))
return (relRow, relCol, relRowMM, relColMM, (knownAngle + math.pi + relTh) % (math.pi * 2))
def hasMarker(self, row, col):
try:
return self.markerLoc.has_key((row, col))
except:
return False
def getMarkerRow(self, name):
return self.markers[name][0]
def getMarkerCol(self, name):
return self.markers[name][1]
def markerExists(self, name):
return self.markers.has_key(name)
def printMarker(self, row, col):
if self.hasMarker(row=row, col=col):
return "%s-%.2f" % (self.markerLoc[(row, col)], self.markerAngle[self.markerLoc[(row, col)]])
else:
return ""
def drawCell(self, row, col, matrix, maxval, path, stepByStep, removeBelow=0):
# see the occupancyGrid redraw comment for more info on the coordinate
# system.
OccupancyGrid.drawCell(self, row, col, matrix, maxval, path, stepByStep, removeBelow)
x = col
y = row
if self.hasMarker(row=row, col=col):
self.canvas.create_text(
(x + 0.5) * self.colScale,
(y + 0.5) * self.rowScale,
tag="label",
text=self.printMarker(row=row, col=col),
anchor="center",
fill="orange",
)
def setRobotLocation(self, robotXMM, robotYMM, robotTh):
self.x_localizeMM = robotXMM
self.y_localizeMM = robotYMM
self.thr_localize = robotTh
self.setCurrent(
row=int(round(self.x_localizeMM / self.rowScaleMM)),
col=int(round(self.y_localizeMM / self.colScaleMM)),
th=self.thr_localize,
)
def updateRobotLocation(self, robot):
# get the list of markers in view
markers = self.markerToolkit.getMarkerData(robot)
# print "I found markers: ",markers
# if we can see at least one marker
if len(markers) >= 1:
# print "localizing using seen markers"
# we look for the closest marker of those found, we will localize
# upon that (assuming less error in measurements the closer the
# marker)
closestMarker = -1
for index in range(len(markers)):
if self.markerExists(markers[index][3]) and (
closestMarker == -1
or
# we assume that negative distance markers are errors
(markers[closestMarker][0] > markers[index][0] and markers[index][0] > 0)
):
closestMarker = index
if closestMarker >= 0:
# print "Localizing using closest marker: ",markers[closestMarker]
# localize sets the current position and orientation of the robot in
# the grid and returns that position as well as more precise MM based
# location information which we will use internally. (remember x=row
# y=col)
coord_robot = self.localize(
markerName=markers[closestMarker][3],
relXOffM=markers[closestMarker][0],
relYOffM=markers[closestMarker][1],
relTh=markers[closestMarker][2],
)
self.x_localizeMM = coord_robot[0]
self.y_localizeMM = coord_robot[1]
self.thr_localize = coord_robot[2]
# temporarily change the robot's unit system to meters
# it seems as though a simulated robot doesn't have a units
# attribute??
try:
oldUnits = robot.units
except AttributeError:
oldUnits = "METERS"
robot.units = "METERS"
# We keep track of where we think that we are and where the robot
# thinks that it is to be able to localize without artoolkit
# in future passes.
self.x_last_marker_localizeMM = self.x_localizeMM
self.x_last_marker_robotMM = robot.x * 1000
self.y_last_marker_localizeMM = self.y_localizeMM
self.y_last_marker_robotMM = robot.y * 1000
self.thr_last_marker_localize = self.thr_localize
self.thr_last_marker_robot = robot.thr
robot.units = oldUnits
# Lastly, we look through all the seen markers and if there are
# any that are NEW we place them in the map (remember that we
# can't localize upon a new marker that is why this comes at the
# end) Note, if we start out with an empty map this will
# place the marker based upon the assumed starting 'current'
# location.
for index in range(len(markers)):
if not self.markerExists(markers[index][3]):
self.placeRelativeMarker(
name=markers[index][3],
relXOffM=markers[index][0],
relYOffM=markers[index][1],
relTh=markers[index][2],
)
# otherwise we localize using odometry
else:
# print "Localizing with odometry"
# temporarily change the robot's unit system to meters
# it seems as though a simulated robot doesn't have a units
# attribute so we try and if not there we just add it.
try:
oldUnits = robot.units
except AttributeError:
oldUnits = "METERS"
robot.units = "METERS"
xDiffMM = (robot.x * 1000) - self.x_last_marker_robotMM
yDiffMM = (robot.y * 1000) - self.y_last_marker_robotMM
thDiff = self.thr_last_marker_robot - self.thr_last_marker_localize
# print "diffs",xDiffMM,yDiffMM,thDiff
# we need to remember that the robot's odometry was set when the
# robot started and thus its coordinate system could be very different,
# most likely even rotated with respect to the map's coordinates.
# also there is a sign difference between the map's Y and the i
# y given by the odometry.
self.x_localizeMM = self.x_last_marker_localizeMM - (
math.cos(thDiff) * xDiffMM + -1 * math.sin(thDiff) * yDiffMM
)
self.y_localizeMM = (
self.y_last_marker_localizeMM + math.sin(thDiff) * xDiffMM + -1 * math.cos(thDiff) * yDiffMM
)
self.thr_localize = robot.thr - self.thr_last_marker_robot + self.thr_last_marker_localize
# print "new location",self.x_localizeMM,self.y_localizeMM,self.thr_localize
self.setCurrent(
row=int(round(self.x_localizeMM / self.rowScaleMM)),
col=int(round(self.y_localizeMM / self.colScaleMM)),
th=self.thr_localize,
)
robot.units = oldUnits
def localize(self, markerName, relXOffM, relYOffM, relTh):
# We are localizing using a marker in the field of vision of the robot.
# relXOffM, relYOffM are the legs of the triangle made from the straight
# line from the robot to the marker (with the base of the right triangle
# along the marker + relTh). relXOffM is along the 'heading' line' and
# relYOffM is at 90 degrees to the heading line
try:
# assume that the marker is in the center of the cell. (I have
# left this as .45 to prevent it from rounding 'up' in other
# places which causes problems with alignemnt.
markerRowMM = (self.getMarkerRow(markerName) + 0.45) * self.rowScaleMM
markerColMM = (self.getMarkerCol(markerName) + 0.45) * self.colScaleMM
markerAngle = self.markerAngle[markerName]
except KeyError:
# we don't have info regarding that marker??
return (-1, -1, -1)
# the return of getPosRelativeToKnown is (row,col,rowMM,colMM,th)
coord_robot = self.getPosRelativeToKnown(markerRowMM, markerColMM, markerAngle, relXOffM, relYOffM, relTh)
# print " marker - row ",self.getMarkerRow(markerName)," col ",\
# self.getMarkerCol(markerName)," th ", markerAngle
# print " - rowMM ",markerRowMM," colMM ", markerColMM
# print " robot - row ",coord_robot[0]," col ",coord_robot[1]," th ", \
# coord_robot[4]
# print " - rowMM ",coord_robot[2]," colMM ", coord_robot[3]
self.setCurrent(row=coord_robot[0], col=coord_robot[1], th=coord_robot[4])
self.redrawTrig.set(1)
# return the location in MM and the th
return (coord_robot[2], coord_robot[3], coord_robot[4])
def calculaVelocidadAvance(self, vgiro):
return 0.75 * (0.1 + 0.9 * (1 - abs(vgiro)))
def determineMove(self, subgoal, sonar):
# note that we assume that the row and col given are subgoals, this
# algorithm can NOT deal with walls or obstacles.
subgoalRow, subgoalCol = subgoal
subgoalXmm = subgoalRow * self.rowScaleMM
subgoalYmm = subgoalCol * self.colScaleMM
angleToGoal = math.atan2((subgoalYmm - self.y_localizeMM) * -1, (subgoalXmm - self.x_localizeMM) * -1)
robotAngleDiff = -normalizeAngle2(angleToGoal - normalizeAngle2(self.thr_localize))
print "DIFF: ", robotAngleDiff
sensores = sonar
# sensores = [2.0*float(sonar[k])/float(max(sonar)) for k in range(len(sonar))]
velgiro = self.calculadorNeuronal.transferencia(sensores + [robotAngleDiff / (2.0 * math.pi)])
velavan = self.calculaVelocidadAvance(velgiro)
return (velavan, velgiro)
"""
def determineMove(self, subgoal):
# note that we assume that the row and col given are subgoals, this
# algorithm can NOT deal with walls or obstacles.
subgoalRow, subgoalCol = subgoal
subgoalXmm = subgoalRow*self.rowScaleMM
subgoalYmm = subgoalCol*self.colScaleMM
# remember that in pyrobot th increases as the robot turns
# COUNTER-CLOCKWISE
# we start by calculating the difference between the current
# heading (orientation angle) of the robot and a straight line to
# the subgoal
angleToGoal = math.atan2((subgoalYmm-self.y_localizeMM)*-1,
(subgoalXmm-self.x_localizeMM)*-1)
robotAngleDiff = normalizeAngle(angleToGoal-self.thr_localize)
#print "Current: ",self.x_localizeMM, self.y_localizeMM, self.thr_localize
#print "Subgoal: ",subgoalXmm, subgoalYmm
#print "angleToGoal",angleToGoal
#print "robotAngleDiff: ",robotAngleDiff
# if the subgoal isn't within an arc of size pi/4 centered in the
# front of the robot we do a hard turn
if (robotAngleDiff > math.pi/8 and robotAngleDiff <= math.pi):
print "hard left"
return(self.NO_FORWARD, self.HARD_LEFT)
elif (robotAngleDiff > math.pi and
robotAngleDiff < (math.pi*2 - (math.pi/8))):
print "hard right"
return(self.NO_FORWARD, self.HARD_RIGHT)
# when within the pi/4 arc we turn less the closer we are to
# having it straight ahead
elif (robotAngleDiff > 0 and robotAngleDiff <= math.pi/8):
print "med-forward, variable to the left"
return(self.MED_FORWARD, robotAngleDiff/math.pi/8)
elif (robotAngleDiff >= (math.pi*2 - (math.pi/8)) and
robotAngleDiff < math.pi*2):
print "med-forward, variable to the right"
return(self.MED_FORWARD, (robotAngleDiff-math.pi*2)/math.pi/8)
# if it is straight ahead to just go straight
else:
print "forward"
return(self.FULL_FORWARD,self.NO_TURN)"""
def mergeOtherGridInfo(self, otherGrid):
# find two common markers
commonKeys = []
for key in self.markers.keys():
if otherGrid.markerExists(key):
commonKeys.append(key)
if len(commonKeys) < 2:
print "I can not find two common markers, merge failed!"
return
# for now we just use the first two common markers found
thisGridAnchor1Row = self.getMarkerRow(commonKeys[0])
thisGridAnchor1Col = self.getMarkerCol(commonKeys[0])
thisGridAnchor2Row = self.getMarkerRow(commonKeys[1])
thisGridAnchor2Col = self.getMarkerCol(commonKeys[1])
otherGridAnchor1Row = otherGrid.getMarkerRow(commonKeys[0])
otherGridAnchor1Col = otherGrid.getMarkerCol(commonKeys[0])
otherGridAnchor2Row = otherGrid.getMarkerRow(commonKeys[1])
otherGridAnchor2Col = otherGrid.getMarkerCol(commonKeys[1])
return OccupancyGrid.mergeOtherGridInfo(
self,
otherGrid,
thisGridAnchor1Row=thisGridAnchor1Row,
thisGridAnchor1Col=thisGridAnchor1Col,
thisGridAnchor2Row=thisGridAnchor2Row,
thisGridAnchor2Col=thisGridAnchor2Col,
otherGridAnchor1Row=otherGridAnchor1Row,
otherGridAnchor1Col=otherGridAnchor1Col,
otherGridAnchor2Row=otherGridAnchor2Row,
otherGridAnchor2Col=otherGridAnchor2Col,
)
def redraw(self, matrix=None, path=None, stepByStep=0, onlyDirty=0):
# The origin of the tkinter coordinate system is at the top left with
# x increasing along the top edge and y increasing along the left
# edge:
#
# 0,0 ---- +x
# |
# |
# +y
#
# This means that x corresponds to the column number and y to the row
# number.
if matrix == None:
matrix = self.grid
maxval = 0.0
for row in range(self.rows):
for col in range(self.cols):
if matrix[row][col] != self.infinity:
maxval = max(matrix[row][col], maxval)
if maxval < 1:
maxval = 1
if onlyDirty:
tmpDirty = self.getAndResetDirtyCells()
while len(tmpDirty) > 0:
(row, col) = tmpDirty.pop(0)
self.drawCell(
row=row, col=col, matrix=matrix, maxval=maxval, path=path, stepByStep=stepByStep, removeBelow=1
)
else:
self.canvas.delete("cell")
self.canvas.delete("label")
for row in range(self.rows):
for col in range(self.cols):
self.drawCell(row=row, col=col, matrix=matrix, maxval=maxval, path=path, stepByStep=stepByStep)
if self.getCurrentRow() >= 0 and self.getCurrentCol() >= 0:
self.canvas.create_text(
(self.getCurrentCol() + 0.5) * self.colScale,
(self.getCurrentRow() + 0.5) * self.rowScale,
tag="cell",
text="Current",
fill="green",
)
if self.getGoalCol() >= 0 and self.getGoalRow() >= 0:
self.canvas.create_text(
(self.getGoalCol() + 0.5) * self.colScale,
(self.getGoalRow() + 0.5) * self.rowScale,
tag="cell",
text="Goal",
fill="green",
)
for k in range(len(self.centros)):
i = self.centros[k][0]
j = self.centros[k][1]
self.canvas.create_text(
(j + 0.5) * self.colScale, (i + 0.5) * self.rowScale, tag="cell", text="Centro " + str(k), fill="blue"
)
def drawCell(self, row, col, matrix, maxval, path, stepByStep, removeBelow=0):
# see the redraw comment for more info on the coordinate system.
x = col
y = row
if removeBelow:
self.canvas.addtag_overlapping(
"toDelete", x * self.colScale, y * self.rowScale, (x + 1) * self.colScale, (y + 1) * self.rowScale
)
self.canvas.delete("toDelete")
if self.background == None or path or stepByStep:
self.canvas.create_rectangle(
x * self.colScale,
y * self.rowScale,
(x + 1) * self.colScale,
(y + 1) * self.rowScale,
width=0,
fill=self.color(matrix[row][col], maxval),
tag="cell",
)
# sys.__stdout__.write("drawing Cell %s,%s with %f/%f\n" % \
# (col,row,matrix[row][col],maxval))
else:
self.canvas.create_rectangle(
x * self.colScale,
y * self.rowScale,
((x + 1) * self.colScale) - 1,
((y + 1) * self.rowScale) - 1,
fill="",
width=1.0,
outline=self.color(matrix[row][col], maxval),
tag="cell",
)
if path and path[row][col] == 1:
self.canvas.create_rectangle(
(x + 0.25) * self.colScale,
(y + 0.25) * self.rowScale,
(x + 0.75) * self.colScale,
(y + 0.75) * self.rowScale,
width=0,
fill="red",
tag="cell",
)
class Navigator(OccupancyGrid):
NO_FORWARD = 0
SLOW_FORWARD = 0.1
MED_FORWARD = 0.5
FULL_FORWARD = 1.0
NO_TURN = 0
MED_LEFT = 0.5
HARD_LEFT = 1.0
MED_RIGHT = -0.5
HARD_RIGHT = -1.0
def __init__(self, cols, rows, widthMM, heightMM,
#the x,y and th of the current location can be passed
currentRow = -1, currentCol = -1, currentTh= 0,
goalRow = -1, goalCol = -1, gridUpdate = "",
image=None, grid=None, gridResize=1.0,showIter=0, title=""):
self.markerToolkit = MarkerToolkit()
self.x_localizeMM = 0
self.y_localizeMM = 0
self.thr_localize = 0
self.x_last_marker_localizeMM = 0
self.x_last_marker_robotMM = 0
self.y_last_marker_localizeMM = 0
self.y_last_marker_robotMM = 0
self.thr_last_marker_localize = 0
self.thr_last_marker_robot = 0
self.resetMarkers()
OccupancyGrid.__init__(self, cols=cols, rows=rows,
widthMM=widthMM, heightMM=heightMM,
currentRow=currentRow, currentCol=currentCol,
currentTh=currentTh,
goalRow=goalRow, goalCol=goalCol,
image=image, grid=grid, gridResize=gridResize,
showIter=showIter, title=title)
def resetMarkers(self):
self.markers = {}
self.markerLoc = {}
self.markerAngle = {}
# we use the same angle convention used in the pyrobot simulator
# angles are expressed in radians, 0 is north and + angles rotate
# counter-clockwise
def placeMarker(self,row,col,name,angle=0,redraw=0):
self.markers[name] = (row,col)
self.markerLoc[(row,col)] = name
self.markerAngle[name] = angle
if (redraw):
self.redrawTrig.set(1);
def placeRelativeMarker(self,name,relXOffM,relYOffM,relTh,redraw=1):
# the -1's are to change the perspective.
newMarkerInfo = self.getPosRelativeToKnown(self.getCurrentRow()*
self.rowScaleMM,
self.getCurrentCol()*
self.colScaleMM,
self.getCurrentTh(),
relXOffM,
relYOffM*-1,
relTh*-1)
print ("placing new marker ",name,newMarkerInfo[0],
newMarkerInfo[1],newMarkerInfo[4])
self.placeMarker(row=newMarkerInfo[0], col=newMarkerInfo[1],
name=name, angle=newMarkerInfo[4], redraw=1)
def getPosRelativeToKnown(self, knownRowMM, knownColMM, knownAngle,
relXOffM, relYOffM, relTh):
#print "relXOffM",relXOffM,"relYOffM",relYOffM,"relTh",relTh,\
# "atan",math.atan(relYOffM/relXOffM),"knownAngle",knownAngle
toKnownDistMM = calculateHypotenuse(relXOffM*1000, relYOffM*1000)
toKnownAngle = knownAngle+relTh
#print "toKnownAngle",toKnownAngle,"toKnownDistMM",toKnownDistMM,\
# "cos-row",math.cos(toKnownAngle),"sin-col",math.sin(toKnownAngle)
#print "rowMMDiff ", (toKnownDistMM * math.cos(toKnownAngle)), \
# "colMMDiff ", (toKnownDistMM * math.sin(toKnownAngle))
relRowMM = knownRowMM - (toKnownDistMM * math.cos(toKnownAngle))
relColMM = knownColMM - (toKnownDistMM * math.sin(toKnownAngle))
relRow = int(round(relRowMM/self.rowScaleMM))
relCol = int(round(relColMM/self.colScaleMM))
return (relRow, relCol, relRowMM, relColMM,
(knownAngle+math.pi+relTh)%(math.pi*2))
def hasMarker(self,row,col):
try:
return self.markerLoc.has_key((row,col))
except:
return False
def getMarkerRow(self,name):
return self.markers[name][0]
def getMarkerCol(self,name):
return self.markers[name][1]
def markerExists(self,name):
return self.markers.has_key(name)
def printMarker(self,row,col):
if self.hasMarker(row=row,col=col):
return "%s-%.2f" % (self.markerLoc[(row,col)],
self.markerAngle[self.markerLoc[(row,col)]])
else:
return ""
def drawCell(self,row,col,matrix,maxval,path,stepByStep,removeBelow=0):
# see the occupancyGrid redraw comment for more info on the coordinate
# system.
OccupancyGrid.drawCell(self,row,col,matrix,maxval,path,stepByStep,
removeBelow)
x = col
y = row
if self.hasMarker(row=row,col=col):
self.canvas.create_text((x + .5) * self.colScale,
(y + .5) * self.rowScale,
tag = 'label',
text=self.printMarker(row=row,col=col),
anchor="center",
fill='orange')
def setRobotLocation(self,robotXMM,robotYMM,robotTh):
self.x_localizeMM = robotXMM
self.y_localizeMM = robotYMM
self.thr_localize = robotTh
self.setCurrent(row=int(round(self.x_localizeMM/self.rowScaleMM)),
col=int(round(self.y_localizeMM/self.colScaleMM)),
th=self.thr_localize)
def updateRobotLocation(self,robot):
# get the list of markers in view
markers = self.markerToolkit.getMarkerData(robot)
# print "I found markers: ",markers
# if we can see at least one marker
if (len(markers) >= 1):
# print "localizing using seen markers"
# we look for the closest marker of those found, we will localize
# upon that (assuming less error in measurements the closer the
# marker)
closestMarker = -1
for index in range(len(markers)):
if (self.markerExists(markers[index][3]) and
(closestMarker == -1 or
# we assume that negative distance markers are errors
(markers[closestMarker][0] > markers[index][0] and
markers[index][0] > 0))):
closestMarker = index
if (closestMarker >= 0):
# print "Localizing using closest marker: ",markers[closestMarker]
# localize sets the current position and orientation of the robot in
# the grid and returns that position as well as more precise MM based
# location information which we will use internally. (remember x=row
# y=col)
coord_robot = self.localize(markerName=markers[closestMarker][3],
relXOffM=markers[closestMarker][0],
relYOffM=markers[closestMarker][1],
relTh=markers[closestMarker][2])
self.x_localizeMM = coord_robot[0]
self.y_localizeMM = coord_robot[1]
self.thr_localize = coord_robot[2]
# temporarily change the robot's unit system to meters
# it seems as though a simulated robot doesn't have a units
# attribute??
try:
oldUnits = robot.units
except AttributeError:
oldUnits = "METERS"
robot.units = "METERS"
# We keep track of where we think that we are and where the robot
# thinks that it is to be able to localize without artoolkit
# in future passes.
self.x_last_marker_localizeMM = self.x_localizeMM
self.x_last_marker_robotMM = robot.x*1000
self.y_last_marker_localizeMM = self.y_localizeMM
self.y_last_marker_robotMM = robot.y*1000
self.thr_last_marker_localize = self.thr_localize
self.thr_last_marker_robot = robot.thr
robot.units = oldUnits
# Lastly, we look through all the seen markers and if there are
# any that are NEW we place them in the map (remember that we
# can't localize upon a new marker that is why this comes at the
# end) Note, if we start out with an empty map this will
# place the marker based upon the assumed starting 'current'
# location.
for index in range(len(markers)):
if (not self.markerExists(markers[index][3])):
self.placeRelativeMarker(name=markers[index][3],
relXOffM=markers[index][0],
relYOffM=markers[index][1],
relTh=markers[index][2])
# otherwise we localize using odometry
else:
# print "Localizing with odometry"
# temporarily change the robot's unit system to meters
# it seems as though a simulated robot doesn't have a units
# attribute so we try and if not there we just add it.
try:
oldUnits = robot.units
except AttributeError:
oldUnits = "METERS"
robot.units = "METERS"
xDiffMM = (robot.x*1000) - self.x_last_marker_robotMM
yDiffMM = (robot.y*1000) - self.y_last_marker_robotMM
thDiff = self.thr_last_marker_robot - self.thr_last_marker_localize
# print "diffs",xDiffMM,yDiffMM,thDiff
# we need to remember that the robot's odometry was set when the
# robot started and thus its coordinate system could be very different,
# most likely even rotated with respect to the map's coordinates.
# also there is a sign difference between the map's Y and the i
# y given by the odometry.
self.x_localizeMM = (self.x_last_marker_localizeMM -
(math.cos(thDiff)*xDiffMM +
-1*math.sin(thDiff)*yDiffMM))
self.y_localizeMM = (self.y_last_marker_localizeMM +
math.sin(thDiff)*xDiffMM +
-1*math.cos(thDiff)*yDiffMM)
self.thr_localize = (robot.thr -
self.thr_last_marker_robot +
self.thr_last_marker_localize)
# print "new location",self.x_localizeMM,self.y_localizeMM,self.thr_localize
self.setCurrent(row=int(round(self.x_localizeMM/self.rowScaleMM)),
col=int(round(self.y_localizeMM/self.colScaleMM)),
th=self.thr_localize)
robot.units = oldUnits
def localize(self, markerName, relXOffM, relYOffM, relTh):
# We are localizing using a marker in the field of vision of the robot.
# relXOffM, relYOffM are the legs of the triangle made from the straight
# line from the robot to the marker (with the base of the right triangle
# along the marker + relTh). relXOffM is along the 'heading' line' and
# relYOffM is at 90 degrees to the heading line
try:
# assume that the marker is in the center of the cell. (I have
# left this as .45 to prevent it from rounding 'up' in other
# places which causes problems with alignemnt.
markerRowMM = (self.getMarkerRow(markerName)+.45)*self.rowScaleMM
markerColMM = (self.getMarkerCol(markerName)+.45)*self.colScaleMM
markerAngle = self.markerAngle[markerName]
except KeyError:
# we don't have info regarding that marker??
return (-1,-1,-1)
#the return of getPosRelativeToKnown is (row,col,rowMM,colMM,th)
coord_robot = self.getPosRelativeToKnown(markerRowMM,
markerColMM,
markerAngle,
relXOffM,
relYOffM, relTh)
#print " marker - row ",self.getMarkerRow(markerName)," col ",\
# self.getMarkerCol(markerName)," th ", markerAngle
#print " - rowMM ",markerRowMM," colMM ", markerColMM
#print " robot - row ",coord_robot[0]," col ",coord_robot[1]," th ", \
# coord_robot[4]
#print " - rowMM ",coord_robot[2]," colMM ", coord_robot[3]
self.setCurrent(row=coord_robot[0],col=coord_robot[1],
th=coord_robot[4])
self.redrawTrig.set(1);
# return the location in MM and the th
return (coord_robot[2], coord_robot[3], coord_robot[4])
def determineMove(self, subgoal):
# note that we assume that the row and col given are subgoals, this
# algorithm can NOT deal with walls or obstacles.
subgoalRow, subgoalCol = subgoal
subgoalXmm = subgoalRow*self.rowScaleMM
subgoalYmm = subgoalCol*self.colScaleMM
# remember that in pyrobot th increases as the robot turns
# COUNTER-CLOCKWISE
# we start by calculating the difference between the current
# heading (orientation angle) of the robot and a straight line to
# the subgoal
angleToGoal = math.atan2((subgoalYmm-self.y_localizeMM)*-1,
(subgoalXmm-self.x_localizeMM)*-1)
robotAngleDiff = normalizeAngle(angleToGoal-self.thr_localize)
#print "Current: ",self.x_localizeMM, self.y_localizeMM, self.thr_localize
#print "Subgoal: ",subgoalXmm, subgoalYmm
#print "angleToGoal",angleToGoal
#print "robotAngleDiff: ",robotAngleDiff
# if the subgoal isn't within an arc of size pi/4 centered in the
# front of the robot we do a hard turn
if (robotAngleDiff > math.pi/8 and robotAngleDiff <= math.pi):
print "hard left"
return(self.NO_FORWARD, self.HARD_LEFT)
elif (robotAngleDiff > math.pi and
robotAngleDiff < (math.pi*2 - (math.pi/8))):
print "hard right"
return(self.NO_FORWARD, self.HARD_RIGHT)
# when within the pi/4 arc we turn less the closer we are to
# having it straight ahead
elif (robotAngleDiff > 0 and robotAngleDiff <= math.pi/8):
print "med-forward, variable to the left"
return(self.MED_FORWARD, robotAngleDiff/math.pi/8)
elif (robotAngleDiff >= (math.pi*2 - (math.pi/8)) and
robotAngleDiff < math.pi*2):
print "med-forward, variable to the right"
return(self.MED_FORWARD, (robotAngleDiff-math.pi*2)/math.pi/8)
# if it is straight ahead to just go straight
else:
print "forward"
return(self.FULL_FORWARD,self.NO_TURN)
"""
This method extracts all the information from another grid to include
it in this grid. It starts by looking for two common markers in this
and the other grid. If 2 can not be found it fails, otherwise it
uses these markers to calculate the transformation between the grids
and then combines the information using this transformation and the
grid update algorithms.
"""
def mergeOtherGridInfo(self, otherGrid):
# find two common markers
commonKeys = []
for key in self.markers.keys():
if (otherGrid.markerExists(key)):
commonKeys.append(key)
if (len(commonKeys) < 2):
print "I can not find two common markers, merge failed!"
return
# for now we just use the first two common markers found
thisGridAnchor1Row = self.getMarkerRow(commonKeys[0])
thisGridAnchor1Col = self.getMarkerCol(commonKeys[0])
thisGridAnchor2Row = self.getMarkerRow(commonKeys[1])
thisGridAnchor2Col = self.getMarkerCol(commonKeys[1])
otherGridAnchor1Row = otherGrid.getMarkerRow(commonKeys[0])
otherGridAnchor1Col = otherGrid.getMarkerCol(commonKeys[0])
otherGridAnchor2Row = otherGrid.getMarkerRow(commonKeys[1])
otherGridAnchor2Col = otherGrid.getMarkerCol(commonKeys[1])
return OccupancyGrid.mergeOtherGridInfo(self,otherGrid,
thisGridAnchor1Row = thisGridAnchor1Row,
thisGridAnchor1Col = thisGridAnchor1Col,
thisGridAnchor2Row = thisGridAnchor2Row,
thisGridAnchor2Col = thisGridAnchor2Col,
otherGridAnchor1Row = otherGridAnchor1Row,
otherGridAnchor1Col = otherGridAnchor1Col,
otherGridAnchor2Row = otherGridAnchor2Row,
otherGridAnchor2Col = otherGridAnchor2Col)