planning.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# {student full name}
# {student id}
# {student email}
from dubins import step
import random
import math

bias = 0.25
b_grid = 0.01

def dis_grid(car, x, y):
   flag = False 
   #if x < car.xlb +b_grid or x > car.xub - b_grid:
      #flag = True
   if y < car.ylb + b_grid or y > car.yub - b_grid:
      flag = True 
   return flag


def find_point(car):    # genera un nuovo punto da esplorare e lo restituisce in una tupla
    point_collide = True
    while point_collide:
        x_srt = random.uniform(car.xlb, car.xub)
        y_srt = random.uniform(car.ylb, car.yub)
        theta_srt = random.uniform(0, 2 * math.pi)
        point_collide = False

        for obs in car.obs:
          ox, oy, r = obs
          distance = math.sqrt((x_srt - ox)**2 + (y_srt - oy)**2)
          if distance <= r + bias or dis_grid(car, x_srt, y_srt):
            point_collide = True
            #print(f"PUNTO SORTEGGIATO COLLIDE")
            #print(f"x casuale", x_srt)
            #print(f"y casuale", y_srt)
            break
    point = (x_srt, y_srt, theta_srt)
    return point

def near_in_tree(tree, point):
    lb = 10000
    nearest = None
    if not tree:
      print(f"ERRORE in near_in_tree: L'albero e' vuoto")
      return None
    
    for tupla in tree:
       if tupla is None: 
          print(f"ERRORE in near_in_tree: una tupla e' vuota")
          return None 
    
    if not point:
      print(f"ERRORE in near_in_tree: il punto e' nullo")

    #ora possiamo partire nel calcolare il vicino 
    
    for node in tree:
        for k in range(len(node)):
            if not isinstance(node[k], float):
              print(f"ERRORE in near_in_tree: elemeno non float in node")
              print(f"il nodo e'", node)
              print(f"il k e'", k)
              return None
        
        for k in range(len(point)):
           if not isinstance(point[k], float):
              print(f"ERRORE in near_in_tree: elemeno non float in point")
        distance = math.sqrt((point[0] - node[0]) ** 2 + (point[1] - node[1]) ** 2)
        if distance < lb:
          lb = distance
          nearest = node
        
    #Ora il nodo piu' vicino e' stato trovato 
    if nearest:
       #print(f"Il piu vicino e'", nearest)
       return nearest
    else:
       print("ERRORE in near_in_tree: il vicino è vuoto")
       return 
    
def compute_control(car, inputs, start, destination):
    lb = 10000
    phi = None
    next = None

    for u in inputs:
        x_new = start[0]
        y_new = start[1]
        theta_new = start[2]
        point_collide = False
        for contatore in range(10):
            x_new, y_new, theta_new = step(car, x_new, y_new, theta_new, u)

            # controlliamo per collisioni
            for obs in car.obs:
                ox, oy, r = obs
                distance = math.sqrt((x_new - ox)**2 + (y_new - oy)**2)
                if distance <= r + bias or dis_grid(car, x_new, y_new):
                    point_collide = True
                    #print(f"PUNTO CALCOLATO COLLIDE")
                    #print(f"x calcolata:", x_new)
                    #print(f"y calcolata:", y_new)
                    #print(f"Xlb:", car.xlb)
                    #print(f"Xub:", car.xub)
                    #print(f"Yub:", car.yub)
                    #print(f"Ylb:", car.xlb)
                    break

        if not point_collide : 
           new = (x_new, y_new, theta_new)  #tupla con nuovo stato
           d = math.sqrt((new[0] - destination[0]) ** 2 + (new[1] - destination[1]) ** 2)
           if d < lb: 
              lb = d
              phi = u
              next = new   
 
    if not phi: 
       #print(f"ERRORE in compute_control: non ho trovato un ingresso")
       return None
    if not next:
       #print(f"ERRORE in compute_control: non ho trovato un successivo")
       return None
    #se sono qua va tutto bene
    return phi, next
            



def solution(car):
  # initial state
  x, y = car.x0, car.y0
  theta = 0.0
  tree = []
  node_base = (x, y, theta)
  tree.append(node_base)
  tolerance = 1.0

  # Crea una tupla con i controlli
  inputs = (-math.pi/4, 0, math.pi/4)

  # Creiamo i vettori da restituire 
  controls = []
  times = [0]
  t = 1

  nodi = []
  nodi.append([node_base, None, 0]) #nodo, padre, controllo

  goal = False 
  p_goal = (car.xt, car.yt, 0.0)
  counter = 0 

  while not goal:
    
    if counter == 30:   #ogni 30 iterazioni prodo a raggiungere il goal
        target = p_goal
        counter = 0
    else: 
        target = find_point(car)

    #print("il nodo selezionato e':", target)
    
    start = near_in_tree(tree, target)
    result = compute_control(car, inputs, start, target)
    if not result:                                          #controllo sul risultato, se e' vuoto devo estrarre un altro punto da esplorare 
      #print(f"ERRORE in solution: result e' None")
      continue
   
    # Il controllo e' positivo, posso assegnare i valori ai risultati 
    control, arrived = result

    # Ulteriore controllo per essere certi di non inserire elementi None nell'albero 
    if not arrived:
       print("ERRORE in solution: siamo arrivati in una posizione None")
       return None 
    else:
       tree.append(arrived)                  #aggiungiamo il nodo all'albero
       nodi.append([arrived, start, control])   #aggiungiamo il nodo e il suo genitore alla lista 
    
    # Ora dobbiamo controllare se ci siamo avvicinati all'obiettivo 
    d_goal = math.sqrt((arrived[0] - p_goal[0]) ** 2 + (arrived[1] - p_goal[1]) ** 2)

    if d_goal < tolerance:
       goal = True
       nodi.append([p_goal, arrived, 0])
    
    #print(f"distanza dal goal:", d_goal)
    counter += 1

  # ora dobbiamo ricostruire il grafo 
  path = []
  print(f"TROVATO")
  print(f"distanza dal goal", d_goal)
  print(len(tree))   

  path.append(nodi[-1][0])
  padre = nodi[-1][1]
  
  #print(f"STO PER RICOSTRURE IL CAMMINO")
  while padre is not node_base: #itero fintanto che il padre non e' None
     flag = False 
     contatore = 0
     #print("SONO QUA")
     while not flag and contatore < len(nodi):
        if padre == nodi[contatore][0] and padre is not None:
           if padre != node_base:
               path.append(nodi[contatore][1])
               #print(f"Ho appeso", padre)
               padre = nodi[contatore][1]
               #print(f"Il nuovo padre e'", padre)
               flag = True 
               for pa in path:
                  if pa is None:
                     print(f"ERRORE HO APPESO NONE")
        else: 
            contatore = contatore + 1 
     if padre is None:
         print(f"Sono uscito")
         break       
  path.reverse() 
  print(f"Ho finito il percorso")
  for nodo in path:  #scorro il cammino partendo dal nodo sorgente 
      #dobbiamo cercare il nodo nella lista nodi per poter ottenere l'azione di controllo
      for i in range(len(nodi)):
         if nodo == nodi[i][0]:
            azione_di_controllo = nodi[i][2] #ho trovato l'azione di controllo 
            break
      for k in range(10):
         controls.append(azione_di_controllo)
         times.append(t * 0.01)
         t = t + 1
  return controls, times