def plot_log_with_map(file_path, map_path):
"""
Read a path log file and plot it
:param file_path: Path log file path
"""
myMap = Map2D(map_path)
robot_locations = []
with open(file_path) as log:
i = csv.reader(log)
for line in i:
x = float(line[0])
y = float(line[1])
th = float(line[2])
robot_locations = robot_locations + [[
int(x * 1000), int(y * 1000),
int(th * 1000)
]]
myMap.drawMapWithRobotLocations(robot_locations, saveSnapshot=True)
def recorrer_camino(self, file_obstaculos, mapa, goals):
f = open(file_obstaculos)
obstaculos = []
for line in f:
line = line.rstrip()
line = line.split(",")
obstaculos.append([int(line[0]), int(line[1]), int(line[2])])
f.close()
myMap = Map2D("mapa1.txt")
# Partes a excluir segun sea la el mapa A o B
parte_izq = [[0, 6], [2, 2]]
parte_central = [[3, 6], [6, 3]]
parte_dch = [[7, 6], [9, 2]]
if mapa == 'A':
parte_dch = [[7, 6], [9, 0]]
elif mapa == 'B':
parte_izq = [[0, 6], [2, 0]]
else:
print("El mapa tiene que ser A o B")
exit(1)
pos = self.readOdometry()
origin = myMap._pos2cell(pos[0], pos[1])
path = myMap.findPath(origin, goals, out_of_grid=[
parte_izq, parte_central, parte_dch])
# Generamos el mapa con los obstaculos aniadidos
mapa_real = Map2D("mapa1.txt")
for obstaculo in obstaculos:
mapa_real.deleteConnection(
obstaculo[0], obstaculo[1], obstaculo[2])
posicion_recorrida = []
# Se va comprobando si el camino se puede realizar sin obstaculos
i = 0
N = len(path)
while i < N:
posicion = path[i]
if i == N-1:
posicion_recorrida.append(posicion)
self.write_log("Alcanzada ultima celda: " +
str(posicion[0])+" "+str(posicion[1]))
if list(posicion) in list(goals):
self.write_log("¡Es un objetivo!")
else:
siguiente = path[i+1]
conexion = arista(posicion, siguiente)
if not mapa_real.isConnected(posicion[0], posicion[1], conexion):
# print("PATH anterior", path)
# Se ha detectado un obstaculo inesperado, se recalcula el camino
myMap.deleteConnection(posicion[0], posicion[1], conexion)
path = myMap.findPath(posicion, goals, out_of_grid=[
parte_izq, parte_central, parte_dch])
# print("PATH posterior", path)
i = 0
N = len(path)
self.write_log("Celda actual: " +
str(posicion[0])+" "+str(posicion[1]))
posicion_recorrida.append(posicion)
i = i+1
# Obtenemos las localizaciones de los robots
localizaciones_robot = []
for i, posicion in enumerate(posicion_recorrida):
if i == len(posicion_recorrida)-1:
x = 200 + 400 * posicion[0]
y = 200 + 400 * posicion[1]
theta = localizaciones_robot[i-1][2]
localizaciones_robot.append([x, y, theta])
self.POS.append([x, y, theta])
else:
siguiente = posicion_recorrida[i+1]
conexion = arista(posicion, siguiente)
x = 200 + 400 * posicion[0]
y = 200 + 400 * posicion[1]
thetas = [90, -1, 0, -1, -90, -1, 180]
theta = thetas[conexion]
localizaciones_robot.append([x, y, math.radians(theta)])
self.POS.append([x, y, math.radians(theta)])
self.x = Value('d', x)
self.y = Value('d', y)
self.th = Value('d', theta)
return myMap
def main(args):
"""
Example to load "mapa1.txt"
"""
try:
if not os.path.isfile(args.mapfile):
print('Map file %s does not exist' % args.mapfile)
exit(1)
map_file = args.mapfile
# 1. load map and compute costs and path
myMap = Map2D(map_file)
# Initialize Odometry
initial_pos = [0.2, 0.2, 0]
robot = Robot(initial_pos)
# Robot logger
start_robot_logger(robot.finished, robot,
"./out/trayectoria_entrega.csv")
# 2. launch updateOdometry thread()
robot.startOdometry()
goal_x = 2
goal_y = 2
myMap.fillCostMatrix([goal_x, goal_y])
route = myMap.planPath([0, 0], [goal_x, goal_y])
robot_locations = []
if is_debug:
start_robot_drawer(robot.finished, robot)
last_reached_pos = [0, 0]
while len(route) > 0:
goal = route.pop(0)
print('Ruta', route)
partial_goal_x = (goal[0] + 0.5) * myMap.sizeCell / 1000.0
partial_goal_y = (goal[1] + 0.5) * myMap.sizeCell / 1000.0
print('Partials: ', partial_goal_x, partial_goal_y)
print('El goal: ', goal)
print('Estoy: ', robot.readOdometry())
no_obstacle = robot.go(partial_goal_x, partial_goal_y)
x_odo, y_odo, th_odo = robot.readOdometry()
if not no_obstacle:
# There are a obstacle
print('Obstacle detected')
x, y, th = myMap.odometry2Cells(x_odo, y_odo, th_odo)
print('ODOMETRIIIA:', x, y, th)
# Delete connections from detected wall
myMap.deleteConnection(int(x), int(y), myMap.rad2Dir(th))
myMap.deleteConnection(int(x), int(y),
(myMap.rad2Dir(th) + 1) % 8)
myMap.deleteConnection(int(x), int(y),
(myMap.rad2Dir(th) - 1) % 8)
route = myMap.replanPath(last_reached_pos[0],
last_reached_pos[1], goal_x, goal_y)
else:
robot_locations.append(
[int(x_odo * 1000),
int(y_odo * 1000),
int(th_odo * 1000)])
last_reached_pos = goal
if last_reached_pos[0] == goal_x and last_reached_pos[1] == goal_y:
print('The goal has been reached')
else:
print('Can\'t reached the goal')
# myMap.drawMapWithRobotLocations(robot_locations)
robot.stopOdometry()
except KeyboardInterrupt:
# except the program gets interrupted by Ctrl+C on the keyboard.
# THIS IS IMPORTANT if we want that motors STOP when we Ctrl+C ...
# robot.stopOdometry()
robot.stopOdometry()
def main(args):
"""
Example to load "mapa1.txt"
"""
try:
if not os.path.isfile(args.mapfile):
print('Map file %s does not exist' % args.mapfile)
exit(1)
map_file = args.mapfile
# Instantiate Odometry with your own files from P2/P3
# robot = Robot()
# ...
# 1. load map and compute costs and path
myMap = Map2D(map_file)
# debugging
myMap.fillCostMatrix([2, 0])
# sample commands to see how to draw the map
sampleRobotLocations = [[0, 0, 0], [600, 600, 3.14]]
# this will save a .png with the current map visualization,
# all robot positions, last one in green
myMap.drawMapWithRobotLocations(sampleRobotLocations)
# this shows the current, and empty, map and an additionally closed connection
#myMap.verbose = True
#myMap.deleteConnection(0, 0, 0)
myMap.drawMap(saveSnapshot=True)
path_map_1 = myMap.findPath(0, 0, 2, 2)
# this will open a window with the results, but does not work well remotely
# sampleRobotLocations = [[200, 200, 3.14 / 2.0], [200, 600, 3.14 / 4.0], [200, 1000, -3.14 / 2.0], ]
# myMap.drawMapWithRobotLocations(sampleRobotLocations)
# 2. launch updateOdometry thread()
# robot.startOdometry()
# ...
# 3. perform trajectory
# robot.setSpeed(1,1) ...
# while (notfinished){
# robot.go(pathX[i],pathY[i]);
# check if there are close obstacles
# deal with them...
# Avoid_obstacle(...) OR RePlanPath(...)
# 4. wrap up and close stuff ...
# This currently unconfigure the sensors, disable the motors,
# and restore the LED to the control of the BrickPi3 firmware.
# robot.stopOdometry()
except KeyboardInterrupt:
# except the program gets interrupted by Ctrl+C on the keyboard.
# THIS IS IMPORTANT if we want that motors STOP when we Ctrl+C ...
# robot.stopOdometry()
print
'do something to stop Robot when we Ctrl+C ...'
def main(args):
try:
MAP_FILE = "mapa.txt"
if not os.path.isfile(MAP_FILE):
print('Map file %s does not exist' % args.mapfile)
exit(1)
myMap = Map2D(MAP_FILE)
if args.option == "B":
robot = Robot(myMap.sizeCell / 1000.0,
option='B',
init_position=[1.8, 0.4, 0.0])
else:
robot = Robot(myMap.sizeCell / 1000.0,
option='A',
init_position=[1.8, 0.4, 0.0])
robot.startOdometry()
DO_S = True
DO_NAV = True
DO_BALL = True
DO_EXIT = True
########## 1. perform S ##########
if DO_S:
print("Empiezo S")
if args.option == "B":
do_B(robot)
else:
do_A(robot)
robot.advance_slightly()
robot.go_center()
pregoal = (6, 1)
x_odom, y_odom, _ = robot.readOdometry()
x_start, y_start = robot.odometria_to_goal(x_odom, y_odom)
robot.go_cell(x_start, y_start, pregoal[0], pregoal[1])
# Centrarse
robot.girar_hasta(0)
# ########## 2. navigate ##########
if DO_NAV:
print("Empiezo navegacion")
start = pregoal
print("start", start)
goal = (2, 5)
myMap.planPath(start[0], start[1], goal[0], goal[1])
goalReached = False
cur_cell = start
while not goalReached:
goalReached, cur_cell = reachGoal(robot, myMap, cur_cell, goal,
myMap.currentPath)
########## 3. search for exit ##########
if DO_EXIT:
print("Empiezo busqueda y salida")
# Girar a -90º para mirar hacia salidas
robot.girar_hasta(180)
## Buscar salida
start = goal
final_exit = robot.findExit()
########## 4. search for ball and capture it ##########
if DO_BALL:
robot.advance_slightly(-0.1)
print("Empiezo busqueda y captura")
ball_reached = robot.trackObject()
if ball_reached:
robot.catch()
########## 5. exit ##########
if DO_EXIT:
robot.go_center()
print("Me he centrado")
x_odom, y_odom, _ = robot.readOdometry()
start = robot.odometria_to_goal(x_odom, y_odom)
myMap.planPath(start[0], start[1], final_exit[0], final_exit[1])
goalReached = False
cur_cell = start
while not goalReached:
goalReached, cur_cell = reachGoal(robot, myMap, cur_cell,
final_exit,
myMap.currentPath)
robot.stopOdometry()
except KeyboardInterrupt:
# except the program gets interrupted by Ctrl+C on the keyboard.
# THIS IS IMPORTANT if we want that motors STOP when we Ctrl+C ...
robot.stopOdometry()
def main(args):
"""
Example to load "mapa1.txt"
"""
primera = True
try:
if not os.path.isfile(args.mapfile):
print('Map file %s does not exist' % args.mapfile)
exit(1)
map_file = args.mapfile
# 1. load map and compute costs and path
myMap = Map2D(map_file)
# TODO START ODOMETRY POR SEPARADO
# SLALOM -> FASE 2
if phase_from <= 2 and 2 <= phase_to:
primera = False
if salida is 'A':
starting_point = coord2Meters((1, 7, -math.pi / 2))
pos1 = (starting_point[0], starting_point[1], math.pi)
pos2 = coord2Meters((1, 5, 0))
pos3 = coord2Meters((1, 3, math.pi))
pos4 = coord2Meters((1, 3, -math.pi / 2))
v = 0.15
w_parado = -math.pi / 8
w_movimiento = 0.375
else: # Salida es B
starting_point = coord2Meters((5, 7, -math.pi / 2))
pos1 = (starting_point[0], starting_point[1], 0)
pos2 = coord2Meters((5, 5, math.pi))
pos3 = coord2Meters((5, 3, 0))
pos4 = coord2Meters((5, 3, math.pi))
v = 0.15
w_parado = math.pi / 8
w_movimiento = -0.375
robot = Robot(starting_point)
# Robot logger
start_robot_logger(robot.finished, robot,
"./out/trayectoria_trabajo_2.csv")
robot.startOdometry()
# Disable sensors
robot.enableProximitySensor(False)
robot.enableGyroSensors(False)
# girar 90
robot.setSpeed(0, w_parado)
robot.wait_for_th(pos1[2], 0.02)
# semicirculo 1
robot.setSpeed(v, w_movimiento)
#robot.wait_for_position(pos2[0], pos2[1], 0.2, False)
wait_for_position(pos2[0], pos2[1], pos2[2], robot, 0.2, 0.02)
# semicirculo 2
robot.setSpeed(v, -w_movimiento)
# robot.wait_for_position(pos3[0], pos3[1], 0.2, False)
wait_for_position(pos3[0], pos3[1], pos3[2], robot, 0.2, 0.02)
# Giro 90 grados mirando al frente
robot.setSpeed(0, 0)
robot.resetOdometry(None, None, math.pi)
robot.setSpeed(0, -w_parado)
robot.wait_for_th(pos4[2], 0.02)
# Me detengo
robot.setSpeed(0, 0)
# LABERINTO -> FASE 3
if phase_from <= 3 and 3 <= phase_to:
if salida is 'A':
starting_point = coord2Meters((1, 3, -math.pi / 2))
init_pos = [1, 3]
goal_x = 3
goal_y = 2
else: # Salida es B
starting_point = coord2Meters((5, 3, -math.pi / 2))
init_pos = [5, 3]
goal_x = 3
goal_y = 2
if primera:
robot = Robot(starting_point)
# Robot logger
start_robot_logger(robot.finished, robot,
"./out/trayectoria_trabajo.csv")
robot.startOdometry()
primera = False
# Disable sensors
# TODO: Enable gyro sensors
robot.enableProximitySensor(True)
robot.enableGyroSensors(True)
print("Salida: ", salida)
myMap.fillCostMatrix([goal_x, goal_y])
route = myMap.planPath([init_pos[0], init_pos[1]],
[goal_x, goal_y])
robot_locations = []
# TODO is debug poner que dibuje
last_reached_pos = [init_pos[0], init_pos[1]]
while len(route) > 0:
goal = route.pop(0)
#print('Ruta', route)
partial_goal_x = (goal[0] + 0.5) * myMap.sizeCell / 1000.0
partial_goal_y = (goal[1] + 0.5) * myMap.sizeCell / 1000.0
#print('Partials: ', partial_goal_x, partial_goal_y)
#print('El goal: ', goal)
#print('Estoy: ', robot.readOdometry())
no_obstacle = robot.go(partial_goal_x, partial_goal_y)
x_odo, y_odo, th_odo = robot.readOdometry()
if not no_obstacle:
# There are a obstacle
print('Obstacle detected')
x, y, th = myMap.odometry2Cells(x_odo, y_odo, th_odo)
#print('ODOMETRIIIA:', x, y, th)
# Delete connections from detected wall
myMap.deleteConnection(int(x), int(y), myMap.rad2Dir(th))
myMap.deleteConnection(int(x), int(y),
(myMap.rad2Dir(th) + 1) % 8)
myMap.deleteConnection(int(x), int(y),
(myMap.rad2Dir(th) - 1) % 8)
route = myMap.replanPath(last_reached_pos[0],
last_reached_pos[1], goal_x,
goal_y)
else:
robot_locations.append([
int(x_odo * 1000),
int(y_odo * 1000),
int(th_odo * 1000)
])
last_reached_pos = goal
if last_reached_pos[0] == goal_x and last_reached_pos[1] == goal_y:
print('The goal has been reached')
else:
print('Can\'t reached the goal')
# ORIENTARSE Y AVANZAR UN POCO PARA DELANTE
# Avanza un poco hacia delante para cruzar la linea de meta
robot.orientate(math.pi / 2)
robot.setSpeed(0.2, 0)
time.sleep(2)
robot.setSpeed(0, 0)
[x, y, th] = robot.readOdometry()
print("Estoy principio 4", x, y, th)
# COGER PELOTA -> FASE 4
if phase_from <= 4 and 4 <= phase_to:
if primera:
if salida is 'A':
robot = Robot(coord2Meters([3, 3, math.pi / 2]))
else:
robot = Robot(coord2Meters([3, 3, math.pi / 2]))
if is_debug:
start_robot_drawer(robot.finished, robot)
else:
start_robot_logger(robot.finished, robot,
"trayectoria_tracking.csv")
# 1. launch updateOdometry thread()
robot.startOdometry()
primera = False
# Disable sensors
# TODO: Enable gyro sensors
robot.enableProximitySensor(False)
robot.enableGyroSensors(False)
redMin = (168, 180, 80)
redMax = (2, 255, 255)
res = robot.trackObject(salida,
colorRangeMin=redMin,
colorRangeMax=redMax)
print('Espero a que la camara se apague')
time.sleep(3) # espera en segundos
print('Supongo que la camara esta apagada')
# RECONOCIMIENTO -> FASE 5
[x, y, th] = robot.readOdometry()
print("Principio de la 5", x, y, th)
if phase_from <= 5 and 5 <= phase_to:
# TODO si es la primera activar odometria y demas
# NO PUEDE SER LA PRIEMRA FASE, TIENE QUE COGER PELOTA PRIMERO
reco = Reco()
if salida is 'A':
turn_speed = 0.1
objective_angle = 7 * math.pi / 8
cell_to_recognize = coord2Meters([4, 6, 0])
cell_to_exit_left = coord2Meters([3, 7, 0])
cell_to_exit_left[0] = cell_to_exit_left[0] - 0.1
cell_to_exit_right_1 = coord2Meters([5, 6, 0])
cell_to_exit_right_2 = coord2Meters([6, 6, 0])
cell_to_exit_right_2[0] = cell_to_exit_right_2[0] + 0.05
cell_to_exit_right_3 = coord2Meters([6, 7, 0])
else:
turn_speed = -0.1
objective_angle = math.pi / 8
cell_to_recognize = coord2Meters([2, 6, 0])
cell_to_exit_left_1 = coord2Meters([1, 6, 0])
cell_to_exit_left_2 = coord2Meters([0, 6, 0])
cell_to_exit_left_3 = coord2Meters([0, 7, 0])
cell_to_exit_right = coord2Meters([3, 7, 0])
robot.enableProximitySensor(True)
robot.orientate(objective_angle)
robot.setSpeed(0, 0)
previous_value = 1000
idem = 0
for i in range(1, 5):
[_, _, new_value] = robot.readSensors()
robot.setSpeed(0, turn_speed)
new_value = 1000
while previous_value >= new_value:
if previous_value == new_value:
idem = idem + 1
else:
idem = 0
previous_value = new_value
[_, _, new_value] = robot.readSensors()
new_value = math.floor(new_value)
print("new value", new_value)
time.sleep(0.1)
idem = idem + 1
print("idem", idem)
robot.setSpeed(0, -turn_speed)
time.sleep(0.1 * 4 * idem / 5)
retro_value = 0.1
time_retro = abs((0.55 - previous_value / 100)) / retro_value
print("tiempo", time_retro)
if previous_value > 55:
robot.setSpeed(retro_value, 0)
else:
robot.setSpeed(-retro_value, 0)
time.sleep(time_retro)
robot.setSpeed(0, 0)
time.sleep(0.3)
if salida == 'A':
robot.resetOdometry(1.8, None, math.pi - 0.001)
else:
robot.resetOdometry(1, None, 0)
[x, y, th] = robot.readOdometry()
print("Ajustadas x e y", x, y, th, math.pi / 2)
robot.orientate((math.pi / 2) - turn_speed)
robot.setSpeed(0, 0)
for i in range(1, 20):
[_, _, previous_value] = robot.readSensors()
print("previous value", previous_value)
robot.resetOdometry(None, 3.2 - previous_value / 100, None)
time_retro = abs((0.55 - previous_value / 100)) / retro_value
"""
print("tiempo",time_retro)
if previous_value > 55:
robot.setSpeed(retro_value, 0)
else:
robot.setSpeed(-retro_value, 0)
time.sleep(time_retro)
"""
robot.setSpeed(0, 0)
print('YA HE PILLADO LA PELOTA Y VOY A: ', cell_to_recognize)
robot.go(cell_to_recognize[0], cell_to_recognize[1])
#print('y me MARCHEEEEE')
# ORIENTARSE HACIA ARRIBA (mirando al frente)
robot.orientate(math.pi / 2)
robot.enableProximitySensor(False)
R2D2 = cv2.imread("reco/R2-D2_s.png", cv2.IMREAD_COLOR)
BB8 = cv2.imread("reco/BB8_s.png", cv2.IMREAD_COLOR)
R2D2_detected, R2D2_points = reco.search_img(R2D2)
BB8_detected, BB8_points = reco.search_img(BB8)
print(R2D2_detected, BB8_detected)
turn_speed = 0.4
advance_time = 3.8
# SALIR POR LA PUERTA CORRESPONDIENTE
if BB8_detected and logo == 'BB8' and salida == 'A':
print('1')
robot.go(cell_to_exit_left[0], cell_to_exit_left[1])
elif R2D2_detected and logo == 'BB8' and salida == 'A':
print('2')
#turn_speed = -turn_speed
#advance_time = advance_time * 2
robot.go(cell_to_exit_right_1[0], cell_to_exit_right_1[1])
robot.go(cell_to_exit_right_2[0], cell_to_exit_right_2[1])
robot.go(cell_to_exit_right_3[0], cell_to_exit_right_3[1])
elif R2D2_detected and logo == 'R2D2' and salida == 'A':
print('3')
robot.go(cell_to_exit_left[0], cell_to_exit_left[1])
elif BB8_detected and logo == 'R2D2' and salida == 'A':
print('4')
robot.go(cell_to_exit_right_1[0], cell_to_exit_right_1[1])
robot.go(cell_to_exit_right_2[0], cell_to_exit_right_2[1])
robot.go(cell_to_exit_right_3[0], cell_to_exit_right_3[1])
turn_speed = -turn_speed
advance_time = advance_time * 2
elif BB8_detected and logo == 'BB8' and salida == 'B':
print('5')
robot.go(cell_to_exit_right[0], cell_to_exit_right[1])
turn_speed = -turn_speed
elif R2D2_detected and logo == 'BB8' and salida == 'B':
print('6')
robot.go(cell_to_exit_left_1[0], cell_to_exit_left_1[1])
robot.go(cell_to_exit_left_2[0], cell_to_exit_left_2[1])
robot.go(cell_to_exit_left_3[0], cell_to_exit_left_3[1])
advance_time = advance_time * 2
elif R2D2_detected and logo == 'R2D2' and salida == 'B':
print('7')
robot.go(cell_to_exit_right[0], cell_to_exit_right[1])
turn_speed = -turn_speed
elif BB8_detected and logo == 'R2D2' and salida == 'B':
print('8')
robot.go(cell_to_exit_left_1[0], cell_to_exit_left_1[1])
robot.go(cell_to_exit_left_2[0], cell_to_exit_left_2[1])
robot.go(cell_to_exit_left_3[0], cell_to_exit_left_3[1])
advance_time = advance_time * 2
elif salida == 'A':
robot.go(cell_to_exit_left[0], cell_to_exit_left[1])
else:
robot.go(cell_to_exit_right[0], cell_to_exit_right[1])
# SPRIIIIINT FINAAAAAL HACIA LA LINEA DE METAAAAA
robot.orientate((math.pi / 2) - 0.1)
robot.setSpeed(0.2, 0)
time.sleep(2.5)
robot.setSpeed(0, 0)
robot.stopOdometry()
except KeyboardInterrupt:
# except the program gets interrupted by Ctrl+C on the keyboard.
# THIS IS IMPORTANT if we want that motors STOP when we Ctrl+C ...
# robot.stopOdometry()
robot.catch("up")
robot.stopOdometry()
reco.stop_camera()