Agregado el módulo de freno de emergencia

cap4/emergency_stop.py

@@ -29,10 +29,21 @@ def mov_duckiebot(key):
 def det_duckie(obs):
     ### DETECTOR HECHO EN LA MISIÓN ANTERIOR
     dets = list()
-
+    lower_yellow = np.array([18, 180, 160])
+    upper_yellow = np.array([39, 255, 255])
+    img_hsv = cv2.cvtColor(obs, cv2.COLOR_RGB2HSV)
+    mask = cv2.inRange(img_hsv, lower_yellow, upper_yellow)
+    img_mask = cv2.bitwise_and(obs, obs, mask = mask)
+    kernel = np.ones((5,5),np.uint8)
+    contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
     for cnt in contours:
-
-        if AREA > min_area:
+        #lower_yellow = np.array([18, 180, 160])
+        #upper_yellow = np.array([39, 255, 255])
+        min_area = 4000
+        x, y, w, h = cv2.boundingRect(cnt)
+        if h*w > min_area:
+            x2 = x + w
+            y2 = y + w
             # En lugar de dibujar, se agrega a la lista
             dets.append((x,y,w,h))
 
@@ -81,7 +92,9 @@ def red_alert(obs):
     duck_pos = np.array([2,0,2])
 
     # Constante que se debe calcular
-    C = 1 # f * dr (f es constante, dr es conocido)
+    f = 790
+    C = f*0.08 # f * dr (f es constante, dr es conocido)
+
 
     while True:
 
@@ -96,30 +109,30 @@ def red_alert(obs):
 
         # Si hay alerta evitar que el Duckiebot avance
         if alert:
-            pass
+            action[0]=np.minimum(0,action[0])
 
         # 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
-
+        dets=det_duckie(obs)
         # Dibuja las detecciones
-
+        draw_dets(obs, dets)
         # 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
+        dist = ((dbot_pos[0]-2)**2+(dbot_pos[2]-2)**2)**0.5
 
         # La alerta se desactiva (opción por defecto)
         alert = False
 
         for d in dets:
             # Alto de la detección en pixeles
-            p = DEFINIR
+            p = d[3]
             # La aproximación se calcula según la fórmula mostrada en la capacitación
-            d_aprox = DEFINIR
+            d_aprox = C/p
 
             # Muestra información relevante
             print('p:', p)
@@ -129,9 +142,9 @@ def red_alert(obs):
             # Si la distancia es muy pequeña activa alerta
             if d_aprox < 0.3:
                 # Activar alarma
-
+                alert = True
                 # Muestra ventana en rojo
-
+                obs = red_alert(obs)
         # Se muestra en una ventana llamada "patos" la observación del simulador
         cv2.imshow('patos', cv2.cvtColor(obs, cv2.COLOR_RGB2BGR))