537 lines
21 KiB
Python
537 lines
21 KiB
Python
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}.")
|
|
|