diff --git a/.gitignore b/.gitignore index 0cabf462..212d1a9f 100644 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,4 @@ Debug_Wayfinding .venv *.onnx checkpoints +*.wbproj diff --git a/scripts/lanch_one_simu.py b/scripts/lanch_one_simu.py new file mode 100644 index 00000000..dd39739a --- /dev/null +++ b/scripts/lanch_one_simu.py @@ -0,0 +1,81 @@ +import os +import sys + +from typing import * +import numpy as np +import onnxruntime as ort + +simu_path = __file__.rsplit('/', 2)[0] + '/src/Simulateur' +if simu_path not in sys.path: + sys.path.insert(0, simu_path) + +from onnx_utils import run_onnx_model +from config import * +from WebotsSimulationGymEnvironment import WebotsSimulationGymEnvironment +from TemporalResNetExtractor import TemporalResNetExtractor +from CNN1DResNetExtractor import CNN1DResNetExtractor +# ------------------------------------------------------------------------- + + + +# --- Chemin vers le fichier ONNX --- + +ONNX_MODEL_PATH = "model.onnx" + +# --- Initialisation du moteur d'inférence ONNX Runtime (ORT) --- +def init_onnx_runtime_session(onnx_path: str) -> ort.InferenceSession: + if not os.path.exists(onnx_path): + raise FileNotFoundError(f"Le fichier ONNX est introuvable à : {onnx_path}. Veuillez l'exporter d'abord.") + + # Crée la session d'inférence + return ort.InferenceSession(onnx_path) #On peut modifier le providers afin de mettre une CUDA + + +if __name__ == "__main__": + if not os.path.exists("/tmp/autotech/"): + os.mkdir("/tmp/autotech/") + + os.system('if [ -n "$(ls /tmp/autotech)" ]; then rm /tmp/autotech/*; fi') + + + # 2. Initialisation de la session ONNX Runtime + try: + ort_session = init_onnx_runtime_session(ONNX_MODEL_PATH) + input_name = ort_session.get_inputs()[0].name + output_name = ort_session.get_outputs()[0].name + print(f"Modèle ONNX chargé depuis {ONNX_MODEL_PATH}") + print(f"Input Name: {input_name}, Output Name: {output_name}") + except FileNotFoundError as e: + print(f"ERREUR : {e}") + print( + "Veuillez vous assurer que vous avez exécuté une fois le script d'entraînement pour exporter 'model.onnx'.") + sys.exit(1) + + # 3. Boucle d'inférence (Test) + env = WebotsSimulationGymEnvironment(0,0) + obs = env.reset() + print("Début de la simulation en mode inférence...") + + max_steps = 5000 + step_count = 0 + + while True: + + action = run_onnx_model(ort_session,obs) + + # 4. Exécuter l'action dans l'environnement + obs, reward, done, info = env.step(action) + + # Note: L'environnement Webots gère généralement son propre affichage + # env.render() # Décommenter si votre env supporte le rendu externe + + # Gestion des fins d'épisodes + if done: + print(f"Épisode(s) terminé(s) après {step_count} étapes.") + obs = env.reset() + + + + # Fermeture propre (très important pour les processus parallèles SubprocVecEnv) + envs.close() + print("Simulation terminée. Environnements fermés.") \ No newline at end of file diff --git a/scripts/launch_train_multiprocessing.py b/scripts/launch_train_multiprocessing.py new file mode 100644 index 00000000..edc13a85 --- /dev/null +++ b/scripts/launch_train_multiprocessing.py @@ -0,0 +1,134 @@ +import logging +import os +import sys + +from typing import * + +import torch.nn as nn + +from stable_baselines3 import PPO +from stable_baselines3.common.vec_env import SubprocVecEnv + +simu_path = __file__.rsplit('/', 2)[0] + '/src/Simulateur' +if simu_path not in sys.path: + sys.path.insert(0, simu_path) + +from Simulateur.config import LOG_LEVEL +from config import * +from TemporalResNetExtractor import TemporalResNetExtractor +from CNN1DResNetExtractor import CNN1DResNetExtractor +from onnx_utils import * + +from WebotsSimulationGymEnvironment import WebotsSimulationGymEnvironment +if LOG_LEVEL == logging.DEBUG: from DynamicActionPlotCallback import DynamicActionPlotDistributionCallback + + +if __name__ == "__main__": + + if not os.path.exists("/tmp/autotech/"): + os.mkdir("/tmp/autotech/") + + os.system('if [ -n "$(ls /tmp/autotech)" ]; then rm /tmp/autotech/*; fi') + + + def make_env(simulation_rank: int, vehicle_rank: int): + if LOG_LEVEL == logging.DEBUG: + print("CAREFUL !!! created an SERVER env with {simulation_rank}_{vehicle_rank}") + return WebotsSimulationGymEnvironment(simulation_rank, vehicle_rank) + + envs = SubprocVecEnv([lambda simulation_rank=simulation_rank, vehicle_rank=vehicle_rank : make_env(simulation_rank, vehicle_rank) for vehicle_rank in range(n_vehicles) for simulation_rank in range(n_simulations)]) + + ExtractorClass = CNN1DResNetExtractor + + policy_kwargs = dict( + features_extractor_class=ExtractorClass, + features_extractor_kwargs=dict( + context_size=context_size, + lidar_horizontal_resolution=lidar_horizontal_resolution, + camera_horizontal_resolution=camera_horizontal_resolution, + device=device + ), + activation_fn=nn.ReLU, + net_arch=[512, 512, 512], + ) + + + ppo_args = dict( + n_steps=4096, + n_epochs=10, + batch_size=256, + learning_rate=3e-4, + gamma=0.99, + verbose=1, + normalize_advantage=True, + device=device + ) + + + save_path = __file__.rsplit("/", 1)[0] + "/checkpoints/" + ExtractorClass.__name__ + "/" + os.makedirs(save_path, exist_ok=True) + + + print(save_path) + print(os.listdir(save_path)) + + valid_files = [x for x in os.listdir(save_path) if x.rstrip(".zip").isnumeric()] + + if valid_files: + model_name = max( + valid_files, + key=lambda x : int(x.rstrip(".zip")) + ) + print(f"Loading model {save_path + model_name}") + model = PPO.load( + save_path + model_name, + envs, + **ppo_args, + policy_kwargs=policy_kwargs + ) + i = int(model_name.rstrip(".zip")) + 1 + print(f"----- Model found, loading {model_name} -----") + + else: + model = PPO( + "MlpPolicy", + envs, + **ppo_args, + policy_kwargs=policy_kwargs + ) + + i = 0 + print("----- Model not found, creating a new one -----") + + print("MODEL HAS HYPER PARAMETERS:") + print(f"{model.learning_rate=}") + print(f"{model.gamma=}") + print(f"{model.verbose=}") + print(f"{model.n_steps=}") + print(f"{model.n_epochs=}") + print(f"{model.batch_size=}") + print(f"{model.device=}") + + print("SERVER : finished executing") + + # obs = envs.reset() + # while True: + # action, _states = model.predict(obs, deterministic=True) # Use deterministic=True for evaluation + # obs, reward, done, info = envs.step(action) + # envs.render() # Optional: visualize the environment + + + while True: + export_onnx(model) + test_onnx(model) + + if LOG_LEVEL <= logging.DEBUG: + model.learn(total_timesteps=500_000, callback=DynamicActionPlotDistributionCallback()) + else: + model.learn(total_timesteps=500_000) + + print("iteration over") + + model.save(save_path + str(i)) + + i += 1 diff --git a/src/Simulateur/WebotsSimulationGymEnvironment.py b/src/Simulateur/WebotsSimulationGymEnvironment.py new file mode 100644 index 00000000..213b0688 --- /dev/null +++ b/src/Simulateur/WebotsSimulationGymEnvironment.py @@ -0,0 +1,106 @@ +import os +from typing import * +import numpy as np +import gymnasium as gym + +from config import * + + +class WebotsSimulationGymEnvironment(gym.Env): + """ + One environment for each vehicle + + n: index of the vehicle + supervisor: the supervisor of the simulation + """ + + def __init__(self, simulation_rank: int, vehicle_rank: int): + + + super().__init__() + self.simulation_rank = simulation_rank + self.vehicle_rank = vehicle_rank + + self.handler = logging.FileHandler(f"/tmp/autotech/Voiture_{self.simulation_rank}_{self.vehicle_rank}.log") + self.handler.setFormatter(FORMATTER) + self.log = logging.getLogger("SERVER") + self.log.setLevel(level=LOG_LEVEL) + self.log.addHandler(self.handler) + + + self.log.info("Initialisation started") + + # this is only true if lidar_horizontal_resolution = camera_horizontal_resolution + box_min = np.zeros([2, context_size, lidar_horizontal_resolution], dtype=np.float32) + box_max = np.ones([2, context_size, lidar_horizontal_resolution], dtype=np.float32) * 30 + + self.observation_space = gym.spaces.Box(box_min, box_max, dtype=np.float32) + self.action_space = gym.spaces.MultiDiscrete([n_actions_steering, n_actions_speed]) + + if not os.path.exists("/tmp/autotech"): + os.mkdir("/tmp/autotech") + + self.log.debug(f"Creation of the pipes") + + os.mkfifo(f"/tmp/autotech/{simulation_rank}_{vehicle_rank}toserver.pipe") + os.mkfifo(f"/tmp/autotech/serverto{simulation_rank}_{vehicle_rank}.pipe") + os.mkfifo(f"/tmp/autotech/{simulation_rank}_{vehicle_rank}tosupervisor.pipe") + + # --mode=fast --minimize --no-rendering --batch --stdout + if vehicle_rank == 0 : + os.system(f""" + webots {__file__.rsplit('/', 1)[0]}/worlds/piste{simulation_rank % n_map}.wbt --mode=fast --minimize --batch --stdout & + echo $! {simulation_rank}_{vehicle_rank} >>/tmp/autotech/simulationranks + """) + + self.log.debug("Connection to the vehicle") + self.fifo_w = open(f"/tmp/autotech/serverto{simulation_rank}_{vehicle_rank}.pipe", "wb") + self.log.debug("Connection to the supervisor") + self.fifo_r = open(f"/tmp/autotech/{simulation_rank}_{vehicle_rank}toserver.pipe", "rb") + + self.log.info("Initialisation finished\n") + + def reset(self, seed=0): + # basically useless function + + # lidar data + # this is true for lidar_horizontal_resolution = camera_horizontal_resolution + self.context = obs = np.zeros([2, context_size, lidar_horizontal_resolution], dtype=np.float32) + info = {} + self.log.info(f"reset finished\n") + return obs, info + + def step(self, action): + + self.log.info("Starting step") + self.log.info(f"sending {action=}") + self.fifo_w.write(action.tobytes()) + self.fifo_w.flush() + + # communication with the supervisor + self.log.debug("trying to get info from supervisor") + cur_state = np.frombuffer(self.fifo_r.read(np.dtype(np.float32).itemsize * (n_sensors + lidar_horizontal_resolution + camera_horizontal_resolution)), dtype=np.float32) + self.log.info(f"received {cur_state=}") + reward = np.frombuffer(self.fifo_r.read(np.dtype(np.float32).itemsize), dtype=np.float32)[0] # scalar + self.log.info(f"received {reward=}") + done = np.frombuffer(self.fifo_r.read(np.dtype(np.bool).itemsize), dtype=np.bool)[0] # scalar + self.log.info(f"received {done=}") + truncated = np.frombuffer(self.fifo_r.read(np.dtype(np.bool).itemsize), dtype=np.bool)[0] # scalar + self.log.info(f"received {truncated=}") + info = {} + + cur_state = np.nan_to_num(cur_state[n_sensors:], nan=0., posinf=30.) + + lidar_obs = cur_state[:lidar_horizontal_resolution] + camera_obs = cur_state[lidar_horizontal_resolution:] + + self.context = obs = np.concatenate([ + self.context[:, 1:], + [lidar_obs[None], camera_obs[None]] + ], axis=1) + + self.log.info("step over") + + return obs, reward, done, truncated, info + + diff --git a/src/Simulateur/__init__.py b/src/Simulateur/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/Simulateur/config.py b/src/Simulateur/config.py index 53400a88..6db6d990 100644 --- a/src/Simulateur/config.py +++ b/src/Simulateur/config.py @@ -1,9 +1,11 @@ # just a file that lets us define some constants that are used in multiple files the simulation from torch.cuda import is_available +import logging + n_map = 2 -n_simulations = 8 -n_vehicles = 1 +n_simulations = 2 +n_vehicles = 2 n_stupid_vehicles = 0 n_actions_steering = 16 n_actions_speed = 16 @@ -11,8 +13,9 @@ lidar_max_range = 12.0 device = "cuda" if is_available() else "cpu" -context_size = 128 -lidar_horizontal_resolution = 128 # DON'T CHANGE THIS VALUE PLS -camera_horizontal_resolution = 128 # DON'T CHANGE THIS VALUE PLS +context_size = 1 +lidar_horizontal_resolution = 1024 # DON'T CHANGE THIS VALUE PLS +camera_horizontal_resolution = 1024 # DON'T CHANGE THIS VALUE PLS -B_DEBUG = False +LOG_LEVEL = logging.DEBUG +FORMATTER = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s') diff --git a/src/Simulateur/controllers/controllerVehicleDriver/controllerVehicleDriver.py b/src/Simulateur/controllers/controllerVehicleDriver/controllerVehicleDriver.py index 1eb6b398..d65b69d7 100644 --- a/src/Simulateur/controllers/controllerVehicleDriver/controllerVehicleDriver.py +++ b/src/Simulateur/controllers/controllerVehicleDriver/controllerVehicleDriver.py @@ -1,8 +1,17 @@ import numpy as np +import psutil import time +import os +import re +import sys +import logging -from vehicle import Driver +# add src/Simulateur to sys.path +path = __file__.rsplit('/', 3)[0] +sys.path.insert(0, path) +from config import * +from vehicle import Driver class VehicleDriver(Driver): """ @@ -13,11 +22,14 @@ class VehicleDriver(Driver): def __init__(self): super().__init__() + basicTimeStep = int(self.getBasicTimeStep()) self.sensorTime = basicTimeStep // 4 - self.i = int(self.getName().split("_")[-1]) + self.v_min = 1 + self.v_max = 9 + self.i = int(self.getName().split("_")[-1]) # Lidar self.lidar = self.getDevice("Hokuyo") self.lidar.enable(self.sensorTime) @@ -32,14 +44,34 @@ def __init__(self): self.touch_sensor.enable(self.sensorTime) # Communication - self.receiver = self.getDevice("TT02_receiver") - self.receiver.enable(self.sensorTime) - self.receiver.setChannel(2 * self.i) # corwe ponds the the supervisor's emitter channel - self.emitter = self.getDevice("TT02_emitter") - self.emitter.setChannel(2 * self.i + 1) # corresponds the the supervisor's receiver channel - # Last data received from the supervisor (steering angle) - self.last_data = np.zeros(2, dtype=np.float32) + proc = psutil.Process(os.getpid()) #current + parent = proc.parent() #parent + grandparent = parent.parent() if parent else None #grandparent + pppid = str(grandparent.pid) + + + self.simulation_rank = int( + re.search( + pppid + r" (\d+)", + open("/tmp/autotech/simulationranks", "r").read(), + re.MULTILINE + ).group(1) + ) + + self.handler = logging.FileHandler(f"/tmp/autotech/Voiture_{self.simulation_rank}_{self.i}.log") + self.handler.setFormatter(FORMATTER) + self.log = logging.getLogger("CLIENT") + self.log.setLevel(level=LOG_LEVEL) + self.log.addHandler(self.handler) + + self.log.debug("Connection to the server") + self.fifo_r = open(f"/tmp/autotech/serverto{self.simulation_rank}_{self.i}.pipe", "rb") + self.log.debug("Connection with the supervisor") + self.fifo_w = open(f"/tmp/autotech/{self.simulation_rank}_{self.i}tosupervisor.pipe", "wb") + + + #Vérification de l"état de la voiture def observe(self): @@ -78,14 +110,22 @@ def observe(self): def step(self): # sends observation to the supervisor - self.emitter.send(self.observe().tobytes()) + # First to be executed + + self.log.info("Starting step") + obs = self.observe() + self.log.info(f"Observe {obs=}") + self.fifo_w.write(obs.tobytes()) + self.fifo_w.flush() + + self.log.debug("Trying to read action from the server") + action = np.frombuffer(self.fifo_r.read(np.dtype(np.int64).itemsize * 2), dtype=np.int64) + self.log.info(f"received {action=}") - if self.receiver.getQueueLength() > 0: - while self.receiver.getQueueLength() > 1: - self.receiver.nextPacket() - self.last_data = np.frombuffer(self.receiver.getBytes(), dtype=np.float32) + # Simulation step - action_steering, action_speed = self.last_data + action_steering = np.linspace(-.4, .4, n_actions_steering, dtype=np.float32)[action[0], None] + action_speed = np.linspace(self.v_min, self.v_max, n_actions_speed, dtype=np.float32)[action[1], None] cur_angle = self.getSteeringAngle() dt = self.getBasicTimeStep() @@ -112,6 +152,7 @@ def run(self): #----------------Programme principal-------------------- def main(): driver = VehicleDriver() + driver.log.info("Starting the vehicle driver\n") driver.run() diff --git a/src/Simulateur/controllers/controllerWorldSupervisor/controllerWorldSupervisor.py b/src/Simulateur/controllers/controllerWorldSupervisor/controllerWorldSupervisor.py index 272fa25f..b0639ce3 100644 --- a/src/Simulateur/controllers/controllerWorldSupervisor/controllerWorldSupervisor.py +++ b/src/Simulateur/controllers/controllerWorldSupervisor/controllerWorldSupervisor.py @@ -7,7 +7,6 @@ from controller import Supervisor supervisor = Supervisor() - import torch.nn as nn import psutil @@ -57,10 +56,6 @@ [-2.01029, -2.51669, 0.0391], ] -def log(s: str): - if B_DEBUG: - print(s, file=open("/tmp/autotech/logs", "a")) - # webots precess launches webots-bin internally who is the process that launches the controllers # that's why we need to get the pppid proc = psutil.Process(os.getpid()) #current @@ -78,11 +73,11 @@ def log(s: str): ) -log(f"CLIENT ?{pppid}? {simulation_rank=}") +print(f"CLIENT ?{pppid}? {simulation_rank=}") -class WebotsVehicleGymEnvironment(gym.Env): +class WebotsVehicleManager: """ One environment for each vehicle @@ -102,46 +97,55 @@ def __init__(self, vehicle_rank: int): # negative value so that the first reset is not skipped self.last_reset = -1e6 - # Emitter - self.emitter = supervisor.getDevice(f"supervisor_emitter_{vehicle_rank}") - self.emitter.setChannel(2 * self.vehicle_rank) + proc = psutil.Process(os.getpid()) #current + parent = proc.parent() #parent + grandparent = parent.parent() if parent else None #grandparent + pppid = str(grandparent.pid) - # Receiver - self.receiver = supervisor.getDevice(f"supervisor_receiver_{vehicle_rank}") - self.receiver.enable(self.sensorTime) - self.receiver.setChannel(2 * self.vehicle_rank + 1) + self.simulation_rank = int( + re.search( + pppid + r" (\d+)", + open("/tmp/autotech/simulationranks", "r").read(), + re.MULTILINE + ).group(1) + ) - log(f"CLIENT{simulation_rank}/{vehicle_rank} : begins init") - log(f"CLIENT{simulation_rank}/{vehicle_rank} : {simulation_rank}toserver.pipe") - self.fifo_w = open(f"/tmp/autotech/{simulation_rank}toserver.pipe", "wb") - log(f"CLIENT{simulation_rank}/{vehicle_rank} : serverto{simulation_rank}.pipe") - self.fifo_r = open(f"/tmp/autotech/serverto{simulation_rank}.pipe", "rb") + self.handler = logging.FileHandler(f"/tmp/autotech/Voiture_{self.simulation_rank}_{self.vehicle_rank}.log") + self.handler.setFormatter(FORMATTER) + self.log = logging.getLogger("SUPERVISOR") + self.log.setLevel(level=LOG_LEVEL) + self.log.addHandler(self.handler) - # Last data received from the car - self.last_data = np.zeros(n_sensors + lidar_horizontal_resolution + camera_horizontal_resolution, dtype=np.float32) + + self.log.debug("Connection to the vehicle") + self.fifo_r = open(f"/tmp/autotech/{self.simulation_rank}_{self.vehicle_rank}tosupervisor.pipe", "rb") + self.log.debug("Connection to the server") + self.fifo_w = open(f"/tmp/autotech/{simulation_rank}_{vehicle_rank}toserver.pipe", "wb") + self.translation_field = supervisor.getFromDef(f"TT02_{self.vehicle_rank}").getField("translation") # may cause access issues ... self.rotation_field = supervisor.getFromDef(f"TT02_{self.vehicle_rank}").getField("rotation") # may cause access issues ... # returns the lidar data of all vehicles def observe(self): - # gets from Receiver - if self.receiver.getQueueLength() > 0: - while self.receiver.getQueueLength() > 1: - self.receiver.nextPacket() - self.last_data = np.frombuffer(self.receiver.getBytes(), dtype=np.float32) - - return self.last_data + # gets from Vehicle + self.log.debug("trying to observe") + obs = np.frombuffer(self.fifo_r.read(np.dtype(np.float32).itemsize * (n_sensors + lidar_horizontal_resolution + camera_horizontal_resolution)), dtype=np.float32) + self.log.info(f"observing {obs=}") + return obs + # reset the gym environment reset def reset(self, seed=None): + self.log.debug("trying to reset vehicle") # this has to be done otherwise thec cars will shiver for a while sometimes when respawning and idk why if supervisor.getTime() - self.last_reset >= 1e-1: + self.log.debug("getting info from vehicle") self.last_reset = supervisor.getTime() vehicle = supervisor.getFromDef(f"TT02_{self.vehicle_rank}") - + self.log.debug("resetting vehicle physics") self.checkpoint_manager.reset(seed) trans = self.checkpoint_manager.getTranslation() rot = self.checkpoint_manager.getRotation() @@ -151,22 +155,23 @@ def reset(self, seed=None): self.checkpoint_manager.update() vehicle.resetPhysics() + self.log.info("vehicle reset done") + - obs = self.observe() + obs = np.zeros(n_sensors + lidar_horizontal_resolution + camera_horizontal_resolution, dtype=np.float32) info = {} - log(f"CLIENT{simulation_rank}/{self.vehicle_rank} : reset over") return obs, info # step function of the gym environment - def step(self, action): - action_steering = np.linspace(-.4, .4, n_actions_steering, dtype=np.float32)[action[0], None] - action_speed = np.linspace(self.v_min, self.v_max, n_actions_speed, dtype=np.float32)[action[1], None] - self.emitter.send(np.array([action_steering, action_speed], dtype=np.float32).tobytes()) + def step(self): + #action_steering = np.linspace(-.4, .4, n_actions_steering, dtype=np.float32)[action[0], None] + #action_speed = np.linspace(self.v_min, self.v_max, n_actions_speed, dtype=np.float32)[action[1], None] # we should add a beacon sensor pointing upwards to detect the beacon + self.log.debug("getting observation") obs = self.observe() + self.log.info(f"observed {obs=}") sensor_data = obs[:n_sensors] - reward = 0 done = np.False_ truncated = np.False_ @@ -191,17 +196,16 @@ def step(self, action): def main(): - envs = [WebotsVehicleGymEnvironment(i) for i in range(n_vehicles)] - log(f"CLIENT ALL : envs created") + envs = [WebotsVehicleManager(i) for i in range(n_vehicles)] + print("CLIENT ALL : envs created") # check_env(env) logdir = "./Webots_tb/" - log(f"CLIENT ALL : {envs=}") supervisor.step() - log("-------------------------------------------------------------------") + for i, e in enumerate(envs): - log(f"CLIENT{simulation_rank}/{e.vehicle_rank} : reset") + e.log.debug("CLIENT : reset") e.reset(i) for i in range(n_vehicles, n_vehicles + n_stupid_vehicles): @@ -215,27 +219,22 @@ def main(): last_moved = [0 for _ in range(n_stupid_vehicles)] while supervisor.step() != -1: - log(f"CLIENT ALL : begin step") + #print("CLIENT ALL : begin step") #Prédiction pour séléctionner une action à partir de l"observation for e in envs: - log(f"CLIENT{simulation_rank}/{e.vehicle_rank} : trying to read from fifo") - action = np.frombuffer(e.fifo_r.read(np.dtype(np.int64).itemsize * 2), dtype=np.int64) - log(f"CLIENT{simulation_rank}/{e.vehicle_rank} : received {action=}") - - obs, reward, done, truncated, info = e.step(action) - - if done: - obs, info = e.reset() + obs, reward, done, truncated, info = e.step() + if done: + obs, info = e.reset() - log(f"CLIENT{simulation_rank}/{e.vehicle_rank} : sending {obs=}") + e.log.info(f"sending {obs=}") e.fifo_w.write(obs.tobytes()) - log(f"CLIENT{simulation_rank}/{e.vehicle_rank} : sending {reward=}") + e.log.info(f"sending {reward=}") e.fifo_w.write(reward.tobytes()) - log(f"CLIENT{simulation_rank}/{e.vehicle_rank} : sending {done=}") + e.log.info(f"sending {done=}") e.fifo_w.write(done.tobytes()) - log(f"CLIENT{simulation_rank}/{e.vehicle_rank} : sending {truncated=}") + e.log.info(f"sending {truncated=}") e.fifo_w.write(truncated.tobytes()) - e.fifo_w.flush() + e.fifo_w.flush() for i in range(n_stupid_vehicles): diff --git a/src/Simulateur/launch_train_multiprocessing.py b/src/Simulateur/launch_train_multiprocessing.py deleted file mode 100644 index f0381c65..00000000 --- a/src/Simulateur/launch_train_multiprocessing.py +++ /dev/null @@ -1,219 +0,0 @@ -import os -import time -from typing import * - -import matplotlib.pyplot as plt -import numpy as np -import torch -import torch.nn as nn -import torch.optim as optim -import torch.multiprocessing as mp - -from stable_baselines3 import PPO -from stable_baselines3.common.env_checker import check_env -from stable_baselines3.common.vec_env import SubprocVecEnv, DummyVecEnv - -import gymnasium as gym - -from onnx_utils import export_onnx, test_onnx -from config import * -from CNN1DExtractor import CNN1DExtractor -from TemporalResNetExtractor import TemporalResNetExtractor -from CNN1DResNetExtractor import CNN1DResNetExtractor - -if B_DEBUG: from DynamicActionPlotCallback import DynamicActionPlotDistributionCallback - - -def log(s: str): - if B_DEBUG: - print(s, file=open("/tmp/autotech/logs", "a")) - -class WebotsSimulationGymEnvironment(gym.Env): - """ - One environment for each vehicle - - n: index of the vehicle - supervisor: the supervisor of the simulation - """ - - def __init__(self, simulation_rank: int): - super().__init__() - self.simulation_rank = simulation_rank - - # this is only true if lidar_horizontal_resolution = camera_horizontal_resolution - box_min = np.zeros([2, context_size, lidar_horizontal_resolution], dtype=np.float32) - box_max = np.ones([2, context_size, lidar_horizontal_resolution], dtype=np.float32) * 30 - - self.observation_space = gym.spaces.Box(box_min, box_max, dtype=np.float32) - self.action_space = gym.spaces.MultiDiscrete([n_actions_steering, n_actions_speed]) - - if not os.path.exists("/tmp/autotech"): - os.mkdir("/tmp/autotech") - - log(f"SERVER{simulation_rank} : {simulation_rank=}") - - os.mkfifo(f"/tmp/autotech/{simulation_rank}toserver.pipe") - os.mkfifo(f"/tmp/autotech/serverto{simulation_rank}.pipe") - - # --mode=fast --minimize --no-rendering --batch --stdout - os.system(f""" - webots {__file__.rsplit('/', 1)[0]}/worlds/piste{simulation_rank % n_map}.wbt --mode=fast --minimize --no-rendering --batch --stdout & - echo $! {simulation_rank} >>/tmp/autotech/simulationranks - """) - log(f"SERVER{simulation_rank} : {simulation_rank}toserver.pipe") - self.fifo_r = open(f"/tmp/autotech/{simulation_rank}toserver.pipe", "rb") - log(f"SERVER{simulation_rank} : serverto{simulation_rank}.pipe") - self.fifo_w = open(f"/tmp/autotech/serverto{simulation_rank}.pipe", "wb") - log(f"SERVER{simulation_rank} : fifo opened :D and init done") - log("-------------------------------------------------------------------") - - def reset(self, seed=0): - # basically useless function - - # lidar data - # this is true for lidar_horizontal_resolution = camera_horizontal_resolution - self.context = obs = np.zeros([2, context_size, lidar_horizontal_resolution], dtype=np.float32) - info = {} - return obs, info - - def step(self, action): - log(f"SERVER{self.simulation_rank} : sending {action=}") - self.fifo_w.write(action.tobytes()) - self.fifo_w.flush() - - # communication with the supervisor - cur_state = np.frombuffer(self.fifo_r.read(np.dtype(np.float32).itemsize * (n_sensors + lidar_horizontal_resolution + camera_horizontal_resolution)), dtype=np.float32) - log(f"SERVER{self.simulation_rank} : received {cur_state=}") - reward = np.frombuffer(self.fifo_r.read(np.dtype(np.float32).itemsize), dtype=np.float32)[0] # scalar - log(f"SERVER{self.simulation_rank} : received {reward=}") - done = np.frombuffer(self.fifo_r.read(np.dtype(np.bool).itemsize), dtype=np.bool)[0] # scalar - log(f"SERVER{self.simulation_rank} : received {done=}") - truncated = np.frombuffer(self.fifo_r.read(np.dtype(np.bool).itemsize), dtype=np.bool)[0] # scalar - log(f"SERVER{self.simulation_rank} : received {truncated=}") - info = {} - - cur_state = np.nan_to_num(cur_state[n_sensors:], nan=0., posinf=30.) - - lidar_obs = cur_state[:lidar_horizontal_resolution] - camera_obs = cur_state[lidar_horizontal_resolution:] - - # apply dropout to the camera - # p = 0.5 - # camera_obs *= np.random.binomial(1, 1-p, camera_obs.shape) # random values in {0, 1} - - self.context = obs = np.concatenate([ - self.context[:, 1:], - [lidar_obs[None], camera_obs[None]] - ], axis=1) - # check if the context is correct - # if self.simulation_rank == 0: - # print(f"{(obs[0] == 0).mean():.3f} {(obs[1] == 0).mean():.3f}") - return obs, reward, done, truncated, info - - -if __name__ == "__main__": - if not os.path.exists("/tmp/autotech/"): - os.mkdir("/tmp/autotech/") - - os.system('if [ -n "$(ls /tmp/autotech)" ]; then rm /tmp/autotech/*; fi') - if B_DEBUG: - print("Webots started", file=open("/tmp/autotech/logs", "w")) - - def make_env(rank: int): - log(f"CAREFUL !!! created an SERVER env with {rank=}") - return WebotsSimulationGymEnvironment(rank) - - envs = SubprocVecEnv([lambda rank=rank : make_env(rank) for rank in range(n_simulations)]) - - ExtractorClass = TemporalResNetExtractor - - policy_kwargs = dict( - features_extractor_class=ExtractorClass, - features_extractor_kwargs=dict( - context_size=context_size, - lidar_horizontal_resolution=lidar_horizontal_resolution, - camera_horizontal_resolution=camera_horizontal_resolution, - device=device - ), - activation_fn=nn.ReLU, - net_arch=[512, 512, 512], - ) - - - ppo_args = dict( - n_steps=4096, - n_epochs=10, - batch_size=256, - learning_rate=3e-4, - gamma=0.99, - verbose=1, - normalize_advantage=True, - device=device - ) - - - save_path = __file__.rsplit("/", 1)[0] + "/checkpoints/" + ExtractorClass.__name__ + "/" - if not os.path.exists(save_path): - os.mkdir(save_path) - - print(save_path) - print(os.listdir(save_path)) - - valid_files = [x for x in os.listdir(save_path) if x.rstrip(".zip").isnumeric()] - - if valid_files: - model_name = max( - valid_files, - key=lambda x : int(x.rstrip(".zip")) - ) - print(f"Loading model {save_path + model_name}") - model = PPO.load( - save_path + model_name, - envs, - **ppo_args, - policy_kwargs=policy_kwargs - ) - i = int(model_name.rstrip(".zip")) + 1 - print(f"----- Model found, loading {model_name} -----") - - else: - model = PPO( - "MlpPolicy", - envs, - **ppo_args, - policy_kwargs=policy_kwargs - ) - - i = 0 - print("----- Model not found, creating a new one -----") - - print("MODEL HAS HYPER PARAMETERS:") - print(f"{model.learning_rate=}") - print(f"{model.gamma=}") - print(f"{model.verbose=}") - print(f"{model.n_steps=}") - print(f"{model.n_epochs=}") - print(f"{model.batch_size=}") - print(f"{model.device=}") - - log(f"SERVER : finished executing") - - # obs = envs.reset() - # while True: - # action, _states = model.predict(obs, deterministic=True) # Use deterministic=True for evaluation - # obs, reward, done, info = envs.step(action) - # envs.render() # Optional: visualize the environment - - - while True: - export_onnx(model) - test_onnx(model) - - if B_DEBUG: - model.learn(total_timesteps=500_000, callback=DynamicActionPlotDistributionCallback()) - else: - model.learn(total_timesteps=500_000) - - model.save(save_path + str(i)) - - i += 1 diff --git a/src/Simulateur/onnx_utils.py b/src/Simulateur/onnx_utils.py index b2ebf87c..7dd7dd5b 100644 --- a/src/Simulateur/onnx_utils.py +++ b/src/Simulateur/onnx_utils.py @@ -3,7 +3,7 @@ import torch.nn as nn import torch from config import * - +import numpy as np from CNN1DExtractor import CNN1DExtractor from TemporalResNetExtractor import TemporalResNetExtractor @@ -38,24 +38,24 @@ def export_onnx(model): model.policy.to(device) model.policy.train() +def run_onnx_model(session : ort.InferenceSession,x : np.ndarray): + + return session.run(None, {"input": x})[0] def test_onnx(model): device = model.policy.device model.policy.eval() true_model = get_true_model(model) + loss_fn = nn.MSELoss() + x = torch.randn(1000, 2, context_size, lidar_horizontal_resolution) + try: session = ort.InferenceSession("model.onnx") except Exception as e: print(f"Error loading ONNX model: {e}") return - def model_onnx(x): - return session.run(None, {"input": x.cpu().numpy()})[0] - - loss_fn = nn.MSELoss() - x = torch.randn(1000, 2, context_size, lidar_horizontal_resolution) - with torch.no_grad(): y_true_test = true_model(x) @@ -63,7 +63,7 @@ def model_onnx(x): y_true_train = true_model(x) true_model.eval() - y_onnx = model_onnx(x) + y_onnx = run_onnx_model(session,x.cpu().numpy()) loss_test = loss_fn(y_true_test, torch.tensor(y_onnx)) loss_train = loss_fn(y_true_train, torch.tensor(y_onnx)) diff --git a/src/Simulateur/worlds/.empty.wbproj b/src/Simulateur/worlds/.empty.wbproj deleted file mode 100644 index 03e2fe31..00000000 --- a/src/Simulateur/worlds/.empty.wbproj +++ /dev/null @@ -1,10 +0,0 @@ -Webots Project File version R2022a -perspectives: 000000ff00000000fd00000002000000010000018d000001bbfc0200000001fb0000001400540065007800740045006400690074006f00720100000015000001bb0000009d00ffffff0000000300000556000000d8fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000005560000005400ffffff000003c3000001bb00000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000001180000032c0100000006010000000100 -sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000200 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: 1 "../Voiture_Autonome/controllers/Voiture_autonome/Voiture_autonome.py" "controllers/Controller_voiture/Controller_voiture.py" -globalOptionalRendering: LidarRaysPaths::LidarPointClouds::DistanceSensorRays -consoles: Console:All:All diff --git a/src/Simulateur/worlds/.piste.wbproj b/src/Simulateur/worlds/.piste.wbproj deleted file mode 100644 index 5d12ca44..00000000 --- a/src/Simulateur/worlds/.piste.wbproj +++ /dev/null @@ -1,9 +0,0 @@ -Webots Project File version R2023b -perspectives: 000000ff00000000fd0000000200000001000001cf00000278fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002780000008900ffffff000000030000078000000176fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000005af0000027800000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000001520000045b0100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000001f000000c0000000fa0100000002010000000200 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: 2 "Bobox.proto" "../../CoVAPSy_Intech/Simulateur/worlds/piste.wbt" "../../CoVAPSy_Intech/Simulateur/protos/Vehicle.proto" -consoles: Console:All:All diff --git a/src/Simulateur/worlds/.piste0.wbproj b/src/Simulateur/worlds/.piste0.wbproj deleted file mode 100644 index 2e84c967..00000000 --- a/src/Simulateur/worlds/.piste0.wbproj +++ /dev/null @@ -1,11 +0,0 @@ -Webots Project File version R2025a -perspectives: 000000ff00000000fd00000002000000010000011c00000177fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000001770000004500ffffff00000003000006c000000216fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000006c00000008700ffffff000006c0000001c400000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000001000000017d0100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000001f0000013e000000fa0100000002010000000200 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: -1 -globalOptionalRendering: LidarRaysPaths::LidarPointClouds -consoles: Console:All:All -renderingDevicePerspectives: TT02_0:RASPI_Camera_V2;1;32;0;0 diff --git a/src/Simulateur/worlds/.piste1.wbproj b/src/Simulateur/worlds/.piste1.wbproj deleted file mode 100644 index 91e2a61f..00000000 --- a/src/Simulateur/worlds/.piste1.wbproj +++ /dev/null @@ -1,13 +0,0 @@ -Webots Project File version R2025a -perspectives: 000000ff00000000fd0000000200000001000000870000028afc0200000001fb0000001400540065007800740045006400690074006f007201000000000000028a0000004500ffffff00000003000006c000000150fc0100000002fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000006c00000008700fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000000000000a000000000000000000000006370000028a00000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff00000001000000020000012b000005a50100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000001e00000364000000fa0100000002010000000200 -minimizedPerspectives: 000000ff00000000fd0000000200000001000000750000017bfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000017b0000003f00ffffff000000030000039b00000039fc0100000002fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000039b0000006900fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000000000000a000000000000000000000003240000017b00000001000000020000000100000008fc00000000 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: -1 -globalOptionalRendering: LidarPointClouds::LidarRaysPaths -consoles: Console:All:All -renderingDevicePerspectives: TT02_0:RASPI_Camera_V2;1;32;0;0 -renderingDevicePerspectives: sparringpartner_car_0:RASPI_Camera_V2;1;32;0;0 diff --git a/src/Simulateur/worlds/.piste2.wbproj b/src/Simulateur/worlds/.piste2.wbproj deleted file mode 100644 index 3568e5a3..00000000 --- a/src/Simulateur/worlds/.piste2.wbproj +++ /dev/null @@ -1,12 +0,0 @@ -Webots Project File version R2025a -perspectives: 000000ff00000000fd0000000200000001000001910000036ffc0200000001fb0000001400540065007800740045006400690074006f007200000000160000036f0000003f00ffffff000000030000039b00000039fc0100000002fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000039b0000006900fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000078000000000000000000000039b0000036a00000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff00000001000000020000014d000006310100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000001d0000013c000000fa0100000002010000000200 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: -1 -globalOptionalRendering: LidarPointClouds::LidarRaysPaths::ContactPoints::CameraFrustums -consoles: Console:All:All -renderingDevicePerspectives: TT02_0:RASPI_Camera_V2;1;32;0;0 -renderingDevicePerspectives: TT02_0:raspi_camera;1;1;0;0 diff --git a/src/Simulateur/worlds/.test_track_2023.wbproj b/src/Simulateur/worlds/.test_track_2023.wbproj deleted file mode 100644 index 3e4d553f..00000000 --- a/src/Simulateur/worlds/.test_track_2023.wbproj +++ /dev/null @@ -1,12 +0,0 @@ -Webots Project File version R2025a -perspectives: 000000ff00000000fd00000002000000010000011c0000017bfc0200000001fb0000001400540065007800740045006400690074006f007200000000160000017b0000008800ffffff000000030000039b00000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000039b0000006900ffffff0000039b0000036800000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000001000000017d0100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000001f00000182000000000100000002010000000200 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: 0 "worlds/test_track_2023.wbt" -consoles: Console:All:All -renderingDevicePerspectives: sparringpartner_car_0:RASPI_Camera_V2;1;32;0;0 -renderingDevicePerspectives: sparringpartner_car_1:RASPI_Camera_V2;1;32;0;0 -renderingDevicePerspectives: sparringpartner_car_2:RASPI_Camera_V2;1;32;0;0 diff --git a/src/Simulateur/worlds/.train_track_2023.wbproj b/src/Simulateur/worlds/.train_track_2023.wbproj deleted file mode 100644 index 27c64582..00000000 --- a/src/Simulateur/worlds/.train_track_2023.wbproj +++ /dev/null @@ -1,11 +0,0 @@ -Webots Project File version R2025a -perspectives: 000000ff00000000fd00000002000000010000011c00000177fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000001770000003f00ffffff000000030000039b00000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000039b0000006900ffffff0000039b0000036800000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000000fc000000580100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000007d0000007d000000000100000002010000000200 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: -1 -consoles: Console:All:All -renderingDevicePerspectives: sparringpartner_car_1:RASPI_Camera_V2;1;32;0;0 -renderingDevicePerspectives: sparringpartner_car_2:RASPI_Camera_V2;1;32;0;0 diff --git a/src/Simulateur/worlds/.train_track_2023_arnaud.wbproj b/src/Simulateur/worlds/.train_track_2023_arnaud.wbproj deleted file mode 100644 index 2e14a753..00000000 --- a/src/Simulateur/worlds/.train_track_2023_arnaud.wbproj +++ /dev/null @@ -1,12 +0,0 @@ -Webots Project File version R2025a -perspectives: 000000ff00000000fd00000002000000010000011c00000177fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000001770000003f00ffffff000000030000039b00000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000039b0000006900ffffff0000039b0000036800000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000000fc0000004f0100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000001f0000013b000000fa0100000002010000000200 -maximizedDockId: -1 -centralWidgetVisible: 1 -orthographicViewHeight: 1 -textFiles: -1 -consoles: Console:All:All -renderingDevicePerspectives: sparringpartner_car_0:RASPI_Camera_V2;1;32;0;0 -renderingDevicePerspectives: sparringpartner_car_1:RASPI_Camera_V2;1;32;0;0 -renderingDevicePerspectives: sparringpartner_car_2:RASPI_Camera_V2;1;32;0;0