524 lines
20 KiB
Python
524 lines
20 KiB
Python
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() |