import os import time import gymnasium as gym import numpy as np import json import paho.mqtt.client as mqtt from stable_baselines3 import PPO from stable_baselines3.common.env_checker import check_env from stable_baselines3.common.callbacks import CheckpointCallback from geopy.distance import geodesic # Caminho para salvar checkpoints checkpoint_dir = "./checkpoints/" model_name = "robot_corridor_model" aprendizado_centralizacao_corredor = False aprendizado_desvio_obstaculos = False aprendizado_aproximacao_proximo_ponto = True aprendizado_alinhamento_com_rua = True # MQTT Configurations BROKER = "localhost" PORT = 1883 STATE_TOPIC = "robot/state" ACTION_TOPIC = "robot/actions" # Global state to be updated via MQTT current_state = None new_state_received = False fator_aproximacao = 0.0 no_corredor = False def on_message(client, userdata, message): global current_state, new_state_received payload = message.payload.decode("utf-8") #print(payload) #print("Recebido") current_state = normalize_keys(json.loads(payload)) new_state_received = True # MQTT Client setup client = mqtt.Client() client.on_message = on_message client.connect(BROKER, PORT) client.subscribe(STATE_TOPIC) client.loop_start() # Utility functions def calculate_distance(lat1, lon1, lat2, lon2): """Calculate the distance in meters between two GPS points.""" return geodesic((lat1, lon1), (lat2, lon2)).meters def calculate_angular_difference(angle1, angle2): """Calculate the absolute angular difference between two angles (0-360).""" diff = (angle2 - angle1 + 360) % 360 return diff if diff <= 180 else 360 - diff def normalize_keys(d): """Normalize dictionary keys to lowercase.""" if isinstance(d, dict): return {k.lower(): normalize_keys(v) for k, v in d.items()} elif isinstance(d, list): return [normalize_keys(i) for i in d] else: return d # Função para encontrar o último checkpoint salvo def get_latest_checkpoint(checkpoint_dir): checkpoints = [os.path.join(checkpoint_dir, f) for f in os.listdir(checkpoint_dir) if f.endswith(".zip")] if not checkpoints: return None return max(checkpoints, key=os.path.getctime) # Retorna o mais recente class RobotEnv(gym.Env): def __init__(self): super(RobotEnv, self).__init__() # Definir limites para normalização/desnormalização self.action_limits = { "speed": (10, 30), # Min e Max "angle": (-30, 30), "movement_type": (0, 1) # Discrete } # Normalizar o espaço de ação para [-1, 1] self.action_space = gym.spaces.Box(low=-1, high=1, shape=(3,), dtype=np.float32) # Define observation space (sem alterações) self.observation_space = gym.spaces.Box( low=-np.inf, high=np.inf, shape=(54,), dtype=np.float32 # Ajuste com base na saída real ) self.state = None self.current_time = 0 self.max_episode_time = 60 self.previous_distance_to_next_point = None def reset(self, seed=None): global new_state_received, current_state, fator_aproximacao fator_aproximacao = 0 self.current_time = 0 new_state_received = False self.previous_distance_to_next_point = None # Reset the previous distance # Capturar o timestamp inicial do episódio self.episode_start_time = time.time() while not new_state_received: pass self.state = self._process_state(current_state) self.state = self._validate_tensor(self.state, name="state") return self.state, {} # Retornar o estado inicial e um dicionário vazio def step(self, action): global new_state_received, current_state if not self.action_space.contains(action): print(f"Ação inválida detectada: {action}") action = np.clip(action, self.action_space.low, self.action_space.high) # Desnormalizar a ação speed = self._denormalize_action(action[0], *self.action_limits["speed"]) angle = self._denormalize_action(action[1], *self.action_limits["angle"]) movement_type = int(self._denormalize_action(action[2], *self.action_limits["movement_type"])) action_payload = json.dumps({ "speed": float(speed), "angle": float(angle), "movementtype": movement_type }) client.publish(ACTION_TOPIC, action_payload) new_state_received = False while not new_state_received: pass self.state = self._process_state(current_state) reward = self._calculate_reward() # Verifica condições de término terminated = False truncated = False # Verificar se o tempo real do episódio excedeu o limite elapsed_time = time.time() - self.episode_start_time if self._reached_end_of_street(): print("Episódio concluído: Final da rua!") efficiency = self.max_episode_time - elapsed_time reward += 50 + (efficiency / self.max_episode_time) * 10 # Recompensa extra por eficiência terminated = True elif self._out_of_bounds(): print("Episódio concluído: Saiu do corredor!") reward -= 20 terminated = True elif self._collided_with_obstacle(): print("Episódio concluído: Colisão!") reward -= 30 terminated = True elif self._out_of_next_point_range(range=5.0): print("Episódio concluído: Fora do alcance do próximo ponto!") reward -= 20 terminated = True elif self._angle_misalignment(threshold=90.0): print("Episódio concluído: Desalinhamento angular excessivo!") reward -= 25 terminated = True elif not np.isfinite(reward): print("Recompensa inválida detectada! Abortando episódio.") reward += 0 truncated = True self.current_time += 1 if elapsed_time >= self.max_episode_time: reward -= 50 truncated = True print("Episódio truncado: Tempo máximo atingido.") print(f"Action: {action_payload}, Reward: {reward}") if (terminated or truncated): #print("Salvando modelo apos 60 segundos.") #if model is not None: # model.save(model_name) action_payload = json.dumps({ "reset": True }) client.publish(ACTION_TOPIC, action_payload) self.state = self._validate_tensor(self.state, name="state") #reward = np.clip(reward, -50, 50) # Retorna os cinco valores exigidos return self.state, reward, terminated, truncated, {} def _process_state(self, state): # Verifica se há obstáculos no estado recebido obstacles = state["current_state"].get("obstacles", []) if obstacles: # Caso existam obstáculos, processa os dados processed_obstacles = np.full((10, 3), -1, dtype=np.float32) # Inicializa com -1 for i, obs in enumerate(obstacles[:10]): # Considere até 10 obstáculos processed_obstacles[i] = [ obs.get("distance", -1), # Distância do obstáculo obs.get("width", -1), # Largura do obstáculo obs.get("sidepercentual", -1) # Posição percentual (0-100%) ] else: # Caso a lista esteja vazia, inicializa com -1 processed_obstacles = np.full((10, 3), -1, dtype=np.float32) # Processa o restante do estado processed_state = { "current_state": { "position": np.array([ state["current_state"]["position"]["latitude"], state["current_state"]["position"]["longitude"], state["current_state"]["position"]["orientation"] ], dtype=np.float32), "speed": np.array([state["current_state"]["control"]["speed"]], dtype=np.float32), "distancetoedges": np.array([ state["current_state"]["distancetoedges"]["left"], state["current_state"]["distancetoedges"]["right"] ], dtype=np.float32), "obstacles": processed_obstacles, "next_point": np.array([ state["current_state"]["nextpoints"][0]["latitude"], state["current_state"]["nextpoints"][0]["longitude"], state["current_state"]["nextpoints"][0]["distance"], state["current_state"]["nextpoints"][0]["orientation"], state["current_state"]["nextpoints"][0]["estimatedtime"] ], dtype=np.float32), "street_end_position": np.array([ state["current_state"]["street"]["street_end_position"]["latitude"], state["current_state"]["street"]["street_end_position"]["longitude"] ], dtype=np.float32), "street_status": np.array([ state["current_state"]["street"]["insidestreet"], state["current_state"]["street"]["edgestreet"], state["current_state"]["street"]["orientation"] ], dtype=np.float32) }, "deltas": { "position_delta": np.array([ state["deltas"]["positiondelta"]["deltalatitude"], state["deltas"]["positiondelta"]["deltalongitude"], state["deltas"]["positiondelta"]["deltapositiondistance"] ], dtype=np.float32), "delta_speed": np.array([state["deltas"]["deltaspeed"]], dtype=np.float32), "delta_angle": np.array([state["deltas"]["deltaangle"]], dtype=np.float32), "delta_distancetoedges": np.array([ state["deltas"]["deltadistancetoedgesleft"], state["deltas"]["deltadistancetoedgesright"] ], dtype=np.float32) }, "time_elapsed": np.array([state["deltas"]["deltatimestamp"]], dtype=np.float32) } # Combine os dados para retornar como vetor achatado combined_state = np.concatenate([ processed_state["current_state"]["position"], processed_state["current_state"]["speed"], processed_state["current_state"]["distancetoedges"], processed_state["current_state"]["obstacles"].flatten(), processed_state["current_state"]["next_point"], processed_state["current_state"]["street_end_position"], processed_state["current_state"]["street_status"], processed_state["deltas"]["position_delta"], processed_state["deltas"]["delta_speed"], processed_state["deltas"]["delta_angle"], processed_state["deltas"]["delta_distancetoedges"], processed_state["time_elapsed"] ]).astype(np.float32) # Converte explicitamente para float32 combined_state = self._validate_tensor(combined_state, name="combined_state") return combined_state def _validate_tensor(self, tensor, name="tensor"): """ Valida o tensor para garantir que ele não contenha valores inválidos (NaN, inf, -inf). Substitui valores inválidos por zeros e avisa sobre o problema. """ if not np.isfinite(tensor).all(): # Verifica se há valores inválidos print(f"Tensor inválido detectado no {name}: {tensor}") tensor = np.nan_to_num(tensor, nan=0.0, posinf=0.0, neginf=0.0) # Substitui NaN/Inf return tensor def _denormalize_action(self, value, min_val, max_val): """Converte um valor de [-1, 1] para [min_val, max_val].""" return (value + 1) * 0.5 * (max_val - min_val) + min_val def _calculate_reward(self): global fator_aproximacao global no_corredor reward = 0 car_angle = self.state[2] # Orientação do robô (em graus) edges = self.state[4:6] # Distância às bordas esquerda e direita obstacles = self.state[6:36].reshape(-1, 3) # Lista de obstáculos current_distance = self.state[38] # Distância ao próximo ponto current_orientation = self.state[39] # Orientacao em relacao ao próximo ponto inside_street = self.state[43] # Indicador "InsideStreet" edge_street = self.state[44] # Indicador "EdgeStreet" street_angle = self.state[45] # Orientação da rua (em graus) # Validação dos vetores edges = self._validate_tensor(edges, name="edges") obstacles = self._validate_tensor(obstacles, name="obstacles") no_corredor = inside_street or edge_street ### 1. Centralização no corredor ### if (aprendizado_centralizacao_corredor): if no_corredor: # Apenas quando no corredor left_dist, right_dist = edges if left_dist > 0 and right_dist > 0: # Dentro dos limites difference = abs(left_dist - right_dist) reward += max(0, 5 - (difference ** 2) * 0.5) print(f"Centralizado no corredor: reward +{max(0, 5 - difference * 2)}, edges={edges}") else: reward -= 5 # Penalidade forte para sair do corredor print(f"Descentralizado ou fora do corredor: reward -5, edges={edges}") ### 2. Obstáculos ### if (aprendizado_desvio_obstaculos): if obstacles.size > 0: # Apenas se houver obstáculos for obs in obstacles: if obs[0] >= 0 and obs[0] < 1.0: # Obstáculo muito próximo reward -= 5 print(f"Obstáculo muito próximo: reward -5, obs={obs}") elif obs[0] >= 0 and (obs[2] < 30 or obs[2] > 70): # Fora da faixa central reward -= 2 print(f"Obstáculo fora da faixa central: reward -2, obs={obs}") ### 3. Aproximação ao próximo ponto ### if (aprendizado_aproximacao_proximo_ponto): # Verifica se estamos se aproximando ou se afastando if self.previous_distance_to_next_point is not None and np.isfinite(self.previous_distance_to_next_point): delta_distance = self.previous_distance_to_next_point - current_distance delta_distance = self._validate_tensor(delta_distance, name="delta_distance") fator_aproximacao = delta_distance # Define o peso com base no contexto (no corredor ou fora) weight = 2 if not no_corredor else 1 # Peso maior fora do corredor rwd = 0 if delta_distance <= 0: # Se afastando do ponto # Penalidade: distância maior ou igual a 5 -> penalidade máxima (-5) # distância menor ou igual a 0 -> penalidade mínima (-1) max_distance = 5.0 min_penalty = -1 max_penalty = -5 rwd = weight * (max_penalty + (max_penalty - min_penalty) * (max_distance - min(current_distance, max_distance)) / max_distance) #print(f"Se afastando do ponto: reward {rwd}, current_distance={current_distance}") else: # Se aproximando do ponto # Recompensa: distância igual a 0 -> recompensa máxima (5) # distância maior ou igual a 5 -> recompensa mínima (1) max_distance = 5.0 min_reward = 1 max_reward = 5 rwd = weight * (min_reward + (max_reward - min_reward) * (max_distance - min(current_distance, max_distance)) / max_distance) #print(f"Se aproximando do ponto: reward {rwd}, current_distance={current_distance}") # Atualiza a recompensa reward += rwd else: print("Primeiro ponto, sem delta_distance.") # Atualiza a distância anterior self.previous_distance_to_next_point = current_distance ### 4. Alinhamento Angular com a Rua ### if aprendizado_alinhamento_com_rua: compare_angle = street_angle if no_corredor else current_orientation car_angle = self._validate_tensor(car_angle, name="car_angle") compare_angle = self._validate_tensor(compare_angle, name="compare_angle") # Calcula a diferença absoluta entre os ângulos angular_difference = np.abs(car_angle - compare_angle) # Ajusta para lidar com o ciclo angular (360 graus) if angular_difference > 180: angular_difference = 360 - angular_difference # Recompensa linear com base no alinhamento angular angular_reward = 5 - (10 * angular_difference / 180) reward += angular_reward ### 5. Validação final da recompensa ### reward = float(self._validate_tensor(np.array([reward], dtype=np.float32), name="reward")[0]) return reward def _reached_end_of_street(self): """ Verifica se o robô chegou no último ponto da rua. """ street_end_lat = self.state[41] street_end_lon = self.state[42] current_lat = self.state[0] current_lon = self.state[1] distance_to_end = calculate_distance(current_lat, current_lon, street_end_lat, street_end_lon) return distance_to_end < 1.0 def _out_of_bounds(self): """ Verifica se o robô invadiu alguma margem de rua. """ global no_corredor if (not aprendizado_centralizacao_corredor): return False edges = self.state[4:6] if no_corredor: return edges[0] < 0 or edges[1] < 0 return False def _collided_with_obstacle(self): """ Verifica se o robô colidiu com algum obstáculo. """ if (not aprendizado_desvio_obstaculos): return False # Extrai os obstáculos do vetor de estado achatado obstacles = self.state[13:43].reshape(-1, 3) # 10 obstáculos, 3 valores cada (distância, largura, lado) # Itera sobre os obstáculos e verifica colisões for obstacle in obstacles: if obstacle[0] > 0 and obstacle[0] < 0.5: # Distância menor que 0.5 indica colisão return True return False # Nenhuma colisão detectada def _out_of_next_point_range(self, range = 5.0): """ Verifica se o robô se afastou muito do próximo ponto. """ global fator_aproximacao, no_corredor fator_limite = -0.0015 if no_corredor else -0.05 #print(fator_aproximacao) if (not aprendizado_aproximacao_proximo_ponto): return False current_distance = self.state[38] # Distância ao próximo ponto if (current_distance > range and fator_aproximacao < fator_limite): # Robo esta a mais de 5 metros do proximo ponto e esta se afastabdo return True return False def _angle_misalignment(self, threshold=30.0): """ Verifica se a diferença angular entre o robô e a rua excede um limite. """ if not aprendizado_alinhamento_com_rua: return False car_angle = self.state[2] # Orientação do robô (em graus) street_angle = self.state[45] # Orientação da rua (em graus) current_orientation = self.state[39] # Orientacao ate o proximo ponto inside_street = self.state[43] # Indicador "InsideStreet" edge_street = self.state[44] # Indicador "EdgeStreet" if not inside_street: threshold = threshold / 2 compare_angle = street_angle if inside_street else current_orientation # Calcula a diferença absoluta entre os ângulo angular_difference = np.abs(car_angle - compare_angle) # Ajusta para o ciclo de 360 graus if angular_difference > 180: angular_difference = 360 - angular_difference # Verifica se a diferença excede o limite if angular_difference > threshold: return True return False # Check the environment env = RobotEnv() check_env(env) # Verifica se o modelo já existe # Verifica se há checkpoints existentes latest_checkpoint = get_latest_checkpoint(checkpoint_dir) if latest_checkpoint: print(f"Carregando o último checkpoint: {latest_checkpoint}") model = PPO.load(latest_checkpoint, env=env, verbose=1, device="cpu") elif os.path.exists(model_name + ".zip"): print("Carregando modelo principal salvo...") model = PPO.load(model_name + ".zip", env=env, verbose=1, device="cpu") else: print("Nenhum modelo encontrado, criando novo...") model = PPO("MlpPolicy", env, verbose=1, device="cpu") # Callback para salvar checkpoints checkpoint_callback = CheckpointCallback(save_freq=500, save_path=checkpoint_dir, name_prefix=model_name) # Continuar o treinamento model.learn(total_timesteps=100000, callback=checkpoint_callback) model.save(f"final_{model_name}") print(f"Treinamento concluído e modelo salvo como final_{model_name}.")