diff --git a/cap4/README.md b/cap4/README.md new file mode 100644 index 0000000..b6e8da2 --- /dev/null +++ b/cap4/README.md @@ -0,0 +1,21 @@ +# Capacitación 4: Visión 3D + +Durante la capacitación 4 se mostraron los conceptos de geometría tanto en 2D como en 3D que se utilizan en la robótica móvil y visión computacional. + +La misión para esta capacitación es programar un freno de emergencia que evite los accidentes en Duckietown. Para esto, deben utilizar el trabajo realizado anteriormente de detectar a los patitos y aplicar la aproximación vista en la capacitación que permite estimar la distancia entre el Duckiebot y los patos. Se agrega el archivo `emergency_stop.py`, el cual posee la estructura principal y funciones de utilidad con sus respectivos comentarios que servirán para poder completar la misión. + +## Funciones útiles +Estas funciones serán de utilidad para resolver el desafío: + +`dets = det_duckie(obs)`: Permite hacer la detección de los patos (deben agregar su detector). Lo importante es que en este caso deben retornar una lista de detecciones! + +`obs = draw_dets(obs, dets)`: Esta función recibe la imagen `obs` en la cual dibuja las detecciones enlistadas en `dets`. + +`img_out = red_alert(img_in)`: Agrega dramatismo al simulador. + +## Mapa para calibración +Además, se adjunta mapa (archivo `free.yaml`) que debe copiarse en la carpeta de mapas de `gym-duckietown`. Este mapa contiene un pato de tamaño conocido (0.08 [m]) en el centro del mapa, cada _tile_ mide 1 [m] y se pueden mover libremente por todo el espacio. + +El código se corre de la siguiente forma: + +`python emergency_stop.py --map-name free` diff --git a/cap4/emergency_stop.py b/cap4/emergency_stop.py new file mode 100644 index 0000000..965365a --- /dev/null +++ b/cap4/emergency_stop.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python + +""" +Este programa implementa un freno de emergencia para evitar accidentes en Duckietown. +""" + +import sys +import argparse +import gym +import gym_duckietown +from gym_duckietown.envs import DuckietownEnv +import numpy as np +import cv2 + +def mov_duckiebot(key): + # La acción de Duckiebot consiste en dos valores: + # velocidad lineal y velocidad de giro + actions = {ord('w'): np.array([1.0, 0.0]), + ord('s'): np.array([-1.0, 0.0]), + ord('a'): np.array([0.0, 1.0]), + ord('d'): np.array([0.0, -1.0]), + ord('q'): np.array([0.3, 1.0]), + ord('e'): np.array([0.3, -1.0]) + } + + action = actions.get(key, np.array([0.0, 0.0])) + return action + +def det_duckie(obs): + ### DETECTOR HECHO EN LA MISIÓN ANTERIOR + dets = list() + + for cnt in contours: + + if AREA > min_area: + # En lugar de dibujar, se agrega a la lista + dets.append((x,y,w,h)) + + return dets + +def draw_dets(obs, dets): + for d in dets: + x1, y1 = d[0], d[1] + x2 = x1 + d[2] + y2 = y1 + d[3] + cv2.rectangle(obs, (int(x1), int(y1)), (int(x2),int(y2)), (0,255,0), 3) + + return obs + +def red_alert(obs): + red_img = np.zeros((480, 640, 3), dtype = np.uint8) + red_img[:,:,0] = 255 + blend = cv2.addWeighted(obs, 0.5, red_img, 0.5, 0) + + return blend + +if __name__ == '__main__': + + # Se leen los argumentos de entrada + parser = argparse.ArgumentParser() + parser.add_argument('--env-name', default="Duckietown-udem1-v1") + parser.add_argument('--map-name', default='udem1') + args = parser.parse_args() + + # Definición del environment + if args.env_name and args.env_name.find('Duckietown') != -1: + env = DuckietownEnv( + map_name = args.map_name, + domain_rand = False, + ) + else: + env = gym.make(args.env_name) + + # Se reinicia el environment + env.reset() + + # Inicialmente no hay alerta + alert = False + + # Posición del pato en el mapa (fija) + duck_pos = np.array([2,0,2]) + + # Constante que se debe calcular + C = 1 # f * dr (f es constante, dr es conocido) + + while True: + + # Captura la tecla que está siendo apretada y almacena su valor en key + key = cv2.waitKey(0) + # Si la tecla es Esc, se sale del loop y termina el programa + if key == 27: + break + + # Se define la acción dada la tecla presionada + action = mov_duckiebot(key) + + # Si hay alerta evitar que el Duckiebot avance + if alert: + pass + + # Se ejecuta la acción definida anteriormente y se retorna la observación (obs), + # la evaluación (reward), etc + obs, reward, done, info = env.step(action) + + # Detección de patos, retorna lista de detecciones + + # Dibuja las detecciones + + # Obtener posición del duckiebot + dbot_pos = env.cur_pos + # Calcular distancia real entre posición del duckiebot y pato + # esta distancia se utiliza para calcular la constante + dist = CALCULAR + + # La alerta se desactiva (opción por defecto) + alert = False + + for d in dets: + # Alto de la detección en pixeles + p = DEFINIR + # La aproximación se calcula según la fórmula mostrada en la capacitación + d_aprox = DEFINIR + + # Muestra información relevante + print('p:', p) + print('Da:', d_aprox) + print('Dr:', dist) + + # Si la distancia es muy pequeña activa alerta + if d_aprox < 0.3: + # Activar alarma + + # Muestra ventana en rojo + + # Se muestra en una ventana llamada "patos" la observación del simulador + cv2.imshow('patos', cv2.cvtColor(obs, cv2.COLOR_RGB2BGR)) + + # Se cierra el environment y termina el programa + env.close()