Máquinas e Tratores Autônomos

import numpy as np
from sklearn.neighbors import KDTree

class AutonomousRoutePlanner:
    def __init__(self, field_map):
        self.field = field_map
        self.obstacles = KDTree(field_map['obstacles'])
        
    def calculate_optimal_route(self, start, end):
        """Calcula a rota ótima evitando obstáculos"""
        path = [start]
        current_pos = start
        
        while np.linalg.norm(current_pos - end) > 0.5:  # 0.5m de precisão
            # Verifica obstáculos em um raio de 5m
            obstacles = self.get_nearby_obstacles(current_pos)
            
            if len(obstacles) > 0:
                # Desvia do obstáculo mais próximo
                current_pos = self.avoid_obstacle(current_pos, end, obstacles)
            else:
                # Move diretamente para o destino
                direction = (end - current_pos) / np.linalg.norm(end - current_pos)
                current_pos += direction * 0.5  # Move 0.5m
            
            path.append(current_pos)
            
        return np.array(path)
    
    def get_nearby_obstacles(self, position):
        """Retorna obstáculos em um raio de 5 metros"""
        indices = self.obstacles.query_radius([position], r=5)
        return self.obstacles.data[indices[0]]