import paho.mqtt.client as mqtt import gymnasium as gym import numpy as np import json from stable_baselines3 import PPO import time from geopy.distance import geodesic # Configurações MQTT BROKER = "localhost" PORT = 1883 STATE_TOPIC = "robot/state" ACTION_TOPIC = "robot/actions" aprendizado_centralizacao_corredor = False aprendizado_desvio_obstaculos = False aprendizado_aproximacao_proximo_ponto = True aprendizado_alinhamento_com_rua = True # Configurações do Modelo MODEL_NAME = "final_robot_corridor_model" # Variáveis Globais para Gerenciar MQTT current_state = None new_state_received = False no_corredor = False fator_aproximacao = 0 first_distance = 0 max_distance = 10 max_angle = 100.0 def calculate_distance(lat1, lon1, lat2, lon2): return geodesic((lat1, lon1), (lat2, lon2)).meters def calculate_angular_difference(angle1, angle2): diff = (angle2 - angle1 + 360) % 360 return diff if diff <= 180 else 360 - diff def normalize_keys(data): if isinstance(data, dict): return {k.lower(): normalize_keys(v) for k, v in data.items()} elif isinstance(data, list): return [normalize_keys(i) for i in data] else: return data def on_message(client, userdata, message): global current_state, new_state_received payload = message.payload.decode("utf-8") current_state = normalize_keys(json.loads(payload)) new_state_received = True # Configuração do cliente MQTT client = mqtt.Client() client.on_message = on_message client.connect(BROKER, PORT) client.subscribe(STATE_TOPIC) client.loop_start() class RobotEnvMQTT(gym.Env): """ Ambiente de teste que utiliza MQTT para comunicação com o simulador. """ def __init__(self): super(RobotEnvMQTT, self).__init__() self.action_space = gym.spaces.Box(low=-1, high=1, shape=(3,), dtype=np.float32) self.observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(54,), dtype=np.float32) # Variáveis internas self.state = None self.max_episode_time = 60 self.previous_distance_to_next_point = None def reset(self): global current_state, new_state_received print("Resetando o ambiente...") # Capturar o timestamp inicial do episódio self.episode_start_time = time.time() new_state_received = False while not new_state_received: time.sleep(0.01) # Aguarda o simulador enviar o estado inicial print("Ambiente de teste resetado.") self.state = self._process_state(current_state) return self.state, {} def step(self, action): global current_state, new_state_received # Envia a ação ao simulador action_payload = { "speed": float(action[0]), # Exemplo: velocidade normalizada "angle": float(action[1]), # Exemplo: ângulo normalizado "movementtype": int(action[2]) # Exemplo: tipo de movimento (0 ou 1) } client.publish(ACTION_TOPIC, json.dumps(action_payload)) # Aguarda o próximo estado new_state_received = False while not new_state_received: time.sleep(0.01) # Processa o estado recebido self.state = self._process_state(current_state) reward = self._calculate_reward() terminated, truncated, add_reward = self._check_termination_conditions() reward += add_reward print(f"Ação: {action_payload}, Recompensa: {reward}, Terminou: {terminated}, Truncado: {truncated}") if (terminated or truncated): action_payload = json.dumps({ "reset": True }) client.publish(ACTION_TOPIC, action_payload) self.state = self._validate_tensor(self.state, name="state") 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 _calculate_reward(self): global fator_aproximacao, no_corredor, max_distance 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) min_penalty = -1 max_penalty = -5 normalized_distance = min(current_distance, max_distance) / max_distance rwd = weight * (max_penalty + (max_penalty - min_penalty) * np.log1p(normalized_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) min_reward = 1 max_reward = 5 normalized_distance = min(current_distance, max_distance) / max_distance rwd = weight * (min_reward + (max_reward - min_reward) * (1 - np.log1p(normalized_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 logarítmica com base no alinhamento angular max_angular_difference = 180.0 normalized_angular_difference = angular_difference / max_angular_difference angular_reward = 5 * (1 - np.log1p(normalized_angular_difference)) 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 _check_termination_conditions(self): global max_distance, max_angle, first_distance terminated = False truncated = False reward = 0 # 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=max_distance): print("Episódio concluído: Fora do alcance do próximo ponto!") reward -= 8 terminated = True elif self._angle_misalignment(threshold=max_angle): print("Episódio concluído: Desalinhamento angular excessivo!") reward -= 9 terminated = True elif not np.isfinite(reward): print("Recompensa inválida detectada! Abortando episódio.") reward += 0 truncated = True # Calcular recompensa extra de acordo com a distancia que o robo parou do ultimo ponto distance_to_end = self._distance_end_of_street() normalized_distance = distance_to_end / first_distance # Recompensa baseada na distância normalizada, escalada para 0 a 10 reward_distance = 10 * np.exp(-normalized_distance * 5.0) reward += reward_distance if elapsed_time >= self.max_episode_time: reward -= 50 truncated = True print("Episódio truncado: Tempo máximo atingido.") return terminated, truncated, reward def _distance_end_of_street(self): """ Verifica a distancia do robo ate o ultimo ponto da rua. """ global first_distance 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) if first_distance == 0: first_distance = distance_to_end return distance_to_end def _reached_end_of_street(self): """ Verifica se o robô chegou no último ponto da rua. """ distance_to_end = self._distance_end_of_street() 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 # Carregar o modelo print(f"Carregando modelo salvo: {MODEL_NAME}.zip") model = PPO.load(MODEL_NAME) # Inicializar o ambiente env = RobotEnvMQTT() # Testar o modelo print("Iniciando testes...") obs, _ = env.reset() done = False total_reward = 0 step_count = 0 while not done: # Aguarda o novo estado antes de prever a próxima ação if new_state_received: # Prever a próxima ação action, _ = model.predict(obs, deterministic=True) # Executar a ação no ambiente MQTT obs, reward, done, truncated, _ = env.step(action) # Incrementar o total de recompensas e contagem de passos total_reward += reward step_count += 1 # Exibir informações do passo print(f"Passo {step_count}: Ação={action}, Recompensa={reward}, Total acumulado={total_reward}") if reward < 0: done = False env.reset() total_reward = 0 print(f"Teste concluído. Total de passos: {step_count}, Recompensa total: {total_reward}") client.loop_stop() client.disconnect()