commit d731240295afe892f027bbdd71ae77ea8bdf7560 Author: wehub-resource-sync Date: Mon Jul 13 12:36:51 2026 +0800 chore: import upstream snapshot with attribution diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0a19790 --- /dev/null +++ b/.gitignore @@ -0,0 +1,174 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# UV +# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +#uv.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/latest/usage/project/#working-with-version-control +.pdm.toml +.pdm-python +.pdm-build/ + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ + +# Ruff stuff: +.ruff_cache/ + +# PyPI configuration file +.pypirc diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md new file mode 100644 index 0000000..6123222 --- /dev/null +++ b/README.md @@ -0,0 +1,225 @@ +๐Ÿš€ [**New Dataset Released !**](#-download-dataset) + +# EasyCarla-RL: A lightweight and beginner-friendly OpenAI Gym environment built on the CARLA simulator + +## Overview + +EasyCarla-RL provides a lightweight and easy-to-use Gym-compatible interface for the CARLA simulator, specifically tailored for reinforcement learning (RL) applications. It integrates essential observation components such as LiDAR scans, ego vehicle states, nearby vehicle information, and waypoints. The environment supports safety-aware learning with reward and cost signals, visualization of waypoints, and customizable parameters including traffic settings, number of vehicles, and sensor range. EasyCarla-RL is designed to help both researchers and beginners efficiently train and evaluate RL agents without heavy engineering overhead. + +
+ + + + + + + +
+ +
+ +## Installation + +Clone the repository: + +```bash +git clone https://github.com/silverwingsbot/EasyCarla-RL.git +cd EasyCarla-RL +``` + +Install the required dependencies: + +```bash +pip install -r requirements.txt +``` + +Install EasyCarla-RL as a local Python package: + +```bash +pip install -e . +``` + +Make sure you have a running [CARLA simulator](https://carla.org/) server compatible with your environment. + +For detailed installation instructions, please refer to the [official CARLA docs](https://carla.readthedocs.io/en/0.9.13/start_quickstart/) + +## Quick Start + +Run a simple demo to interact with the environment: + +```bash +python easycarla_demo.py +``` + +This script demonstrates how to: +- Create and reset the environment +- Select random or autopilot actions +- Step through the environment and receive observations, rewards, costs, and done signals + +Make sure your CARLA server is running before executing the demo. + +## Advanced Example: Evaluation with Diffusion Q-Learning + +For a more advanced usage, you can run a pre-trained [Diffusion Q-Learning](https://github.com/Zhendong-Wang/Diffusion-Policies-for-Offline-RL) agent in the EasyCarla-RL environment: + +```bash +cd example +python run_dql_in_carla.py +``` + +Make sure you have downloaded or prepared a trained model checkpoint under the `example/params_dql/` directory. + +This example demonstrates: +- Loading a pre-trained RL agent +- Interacting with EasyCarla-RL for evaluation +- Evaluating the performance of a real RL model on a simulated autonomous driving task + +## ๐Ÿ“ฅ Download Dataset + +This repository provides an offline dataset for training and evaluating RL agents in the EasyCarla-RL environment. + +This dataset includes over **7,000 trajectories** and **1.1 million timesteps**, collected from a mix of expert and random policies (with an **8:2 ratio** of expert to random), recorded in the Town03 map. The data is stored in **HDF5 format**. + +You can download it from either of the following sources: + +* [Download from Hugging Face (direct link)](https://huggingface.co/datasets/silverwingsbot/easycarla/resolve/main/easycarla_offline_dataset.hdf5) +* [Download from ็™พๅบฆ็ฝ‘็›˜ (ๆๅ–็ : 2049)](https://pan.baidu.com/s/1yhCFzl4RFHzxfszebYnOIg?pwd=2049) + +Filename: `easycarla_offline_dataset.hdf5` Size: \~2.76 GB Format: HDF5 + +### Dataset Structure (HDF5) + +Each sample in the dataset includes the following fields: + +``` +/ (root) +โ”œโ”€โ”€ observations โ†’ shape: [N, 307] # concatenated: ego_state + lane_info + lidar + nearby_vehicles + waypoints +โ”œโ”€โ”€ actions โ†’ shape: [N, 3] # [throttle, steer, brake] +โ”œโ”€โ”€ rewards โ†’ shape: [N] # scalar reward per step +โ”œโ”€โ”€ costs โ†’ shape: [N] # safety-related cost signal per step +โ”œโ”€โ”€ done โ†’ shape: [N] # 1 if episode ends +โ”œโ”€โ”€ next_observations โ†’ shape: [N, 307] # next-step observations, same format as observations +โ”œโ”€โ”€ info โ†’ dict containing: +โ”‚ โ”œโ”€โ”€ is_collision โ†’ shape: [N] # 1 if collision occurs +โ”‚ โ””โ”€โ”€ is_off_road โ†’ shape: [N] # 1 if vehicle leaves the road +``` + +* `N` is the number of total timesteps across all episodes (\~1.1 million). +* `observations` and `next_observations` are 307-dimensional vectors formed by concatenating: + + * `ego_state` (9) + `lane_info` (2) + `lidar` (240) + `nearby_vehicles` (20) + `waypoints` (36) + +### Observation Format + +Each observation in the dataset is stored as a **307-dimensional flat vector**, constructed by concatenating several components in the following order: + +```python +# Flattening function used during data generation + +def flatten_obs(obs_dict): + return np.concatenate([ + obs_dict['ego_state'], # 9 dimensions + obs_dict['lane_info'], # 2 dimensions + obs_dict['lidar'], # 240 dimensions + obs_dict['nearby_vehicles'], # 20 dimensions + obs_dict['waypoints'] # 36 dimensions + ]).astype(np.float32) # Total: 307 dimensions +``` + +This format allows for efficient training of neural networks while preserving critical spatial and semantic information. + +### How to Load and Train with HDF5 Dataset๏ผŸ + +This example shows how to load the offline dataset and use it in a typical RL training loop. The model here is a placeholder โ€” you can plug in any behavior cloning, Q-learning, or actor-critic model. + +```python +import h5py +import torch +import numpy as np + +# === Load dataset from HDF5 === +with h5py.File('easycarla_offline_dataset.hdf5', 'r') as f: + observations = torch.tensor(f['observations'][:], dtype=torch.float32) + actions = torch.tensor(f['actions'][:], dtype=torch.float32) + rewards = torch.tensor(f['rewards'][:], dtype=torch.float32) + next_observations = torch.tensor(f['next_observations'][:], dtype=torch.float32) + dones = torch.tensor(f['done'][:], dtype=torch.float32) + +# === (Optional) check shape info === +print("observations:", observations.shape) +print("actions:", actions.shape) + +# === Placeholder model example === +class YourModel(torch.nn.Module): + def __init__(self, obs_dim, act_dim): + super().__init__() + # define your model here + pass + + def forward(self, obs): + # define forward pass + return None + +# === Training setup === +model = YourModel(obs_dim=observations.shape[1], act_dim=actions.shape[1]) +optimizer = torch.optim.Adam(model.parameters(), lr=3e-4) +loss_fn = torch.nn.MSELoss() + +# === Offline RL training loop === +for epoch in range(1, 11): # e.g. 10 epochs + for step in range(100): # e.g. 100 steps per epoch + # sample random batch + idx = np.random.randint(0, len(observations), size=256) + obs_batch = observations[idx] + act_batch = actions[idx] + rew_batch = rewards[idx] + next_obs_batch = next_observations[idx] + done_batch = dones[idx] + + # forward, compute loss + pred = model(obs_batch) # e.g. predict action or Q-value + loss = loss_fn(pred, act_batch) # just an example + + # backward and update + optimizer.zero_grad() + loss.backward() + optimizer.step() + + print(f"[Epoch {epoch}] Loss: {loss.item():.4f} # Replace with your own logging or evaluation") +``` + +## Project Structure + +``` +EasyCarla-RL/ +โ”œโ”€โ”€ easycarla/ # Main environment module (Python package) +โ”‚ โ”œโ”€โ”€ envs/ +โ”‚ โ”‚ โ”œโ”€โ”€ __init__.py +โ”‚ โ”‚ โ””โ”€โ”€ carla_env.py # Carla environment wrapper following the Gym API +โ”‚ โ””โ”€โ”€ __init__.py +โ”œโ”€โ”€ example/ # Advanced example +โ”‚ โ”œโ”€โ”€ agents/ +โ”‚ โ”œโ”€โ”€ params_dql/ +โ”‚ โ”œโ”€โ”€ utils/ +โ”‚ โ””โ”€โ”€ run_dql_in_carla.py # Script to run a pretrained RL model +โ”œโ”€โ”€ easycarla_demo.py # Quick Start demo script (basic Gym-style environment interaction) +โ”œโ”€โ”€ requirements.txt +โ”œโ”€โ”€ setup.py +โ””โ”€โ”€ README.md +``` + +## License + +This project is licensed under the [Apache License 2.0](https://www.apache.org/licenses/LICENSE-2.0). + +## Author + +Created by [SilverWings](https://github.com/silverwingsbot) + +## ๐Ÿ’“ Acknowledgement + +This project is made possible thanks to the following outstanding open-source contributions: + +- [CARLA](https://github.com/carla-simulator/carla) +- [gym-carla](https://github.com/cjy1992/gym-carla) +- [Diffusion Q-Learning](https://github.com/Zhendong-Wang/Diffusion-Policies-for-Offline-RL) diff --git a/README.wehub.md b/README.wehub.md new file mode 100644 index 0000000..ffafe7c --- /dev/null +++ b/README.wehub.md @@ -0,0 +1,7 @@ +# WeHub ๆฅๆบ่ฏดๆ˜Ž + +- ๅŽŸๅง‹้กน็›ฎ๏ผš`silverwingsbot/EasyCarla-RL` +- ๅŽŸๅง‹ไป“ๅบ“๏ผšhttps://github.com/silverwingsbot/EasyCarla-RL +- ๅฏผๅ…ฅๆ–นๅผ๏ผšไธŠๆธธ้ป˜่ฎคๅˆ†ๆ”ฏ็š„ๆœ€ๆ–ฐๅฟซ็…ง +- ๅŽŸไฝœ่€…ใ€็‰ˆๆƒๅ’Œ่ฎธๅฏ่ฏไฟกๆฏไปฅๅŽŸๅง‹ไป“ๅบ“ๅŠๆœฌไป“ๅบ“ LICENSE ไธบๅ‡† +- ๆœฌๆ–‡ไปถไป…็”จไบŽ่ฎฐๅฝ•ๆฅๆบ๏ผŒไธไปฃ่กจ WeHub ๆ˜ฏๅŽŸ้กน็›ฎไฝœ่€… diff --git a/assets/part1.gif b/assets/part1.gif new file mode 100644 index 0000000..125a594 Binary files /dev/null and b/assets/part1.gif differ diff --git a/assets/part2.gif b/assets/part2.gif new file mode 100644 index 0000000..4d9e11c Binary files /dev/null and b/assets/part2.gif differ diff --git a/assets/part3.gif b/assets/part3.gif new file mode 100644 index 0000000..decd894 Binary files /dev/null and b/assets/part3.gif differ diff --git a/easycarla/__init__.py b/easycarla/__init__.py new file mode 100644 index 0000000..c0a3587 --- /dev/null +++ b/easycarla/__init__.py @@ -0,0 +1,6 @@ +from gym.envs.registration import register + +register( + id='carla-v0', + entry_point='easycarla.envs:CarlaEnv', +) \ No newline at end of file diff --git a/easycarla/envs/__init__.py b/easycarla/envs/__init__.py new file mode 100644 index 0000000..a632443 --- /dev/null +++ b/easycarla/envs/__init__.py @@ -0,0 +1 @@ +from easycarla.envs.carla_env import CarlaEnv diff --git a/easycarla/envs/carla_env.py b/easycarla/envs/carla_env.py new file mode 100644 index 0000000..b04dfe2 --- /dev/null +++ b/easycarla/envs/carla_env.py @@ -0,0 +1,846 @@ +# -*- coding: utf-8 -*- +""" +Author: SilverWings +GitHub: https://github.com/silverwingsbot +""" + +from __future__ import division +import numpy as np +import random +import time +import gym +from gym import spaces +from gym.utils import seeding +import carla + +class CarlaEnv(gym.Env): + def __init__(self, params): + self.collision_sensor = None + self.lidar_sensor = None + self._is_collision = False + self._is_off_road = False + self.off_road_counter = 0 + self.number_of_vehicles = params['number_of_vehicles'] + self.number_of_walkers = params['number_of_walkers'] + self.dt = params['dt'] + self.max_time_episode = params['max_time_episode'] + self.max_waypoints = params['max_waypoints'] + self.visualize_waypoints = params['visualize_waypoints'] + self.desired_speed = params['desired_speed'] + self.max_ego_spawn_times = params['max_ego_spawn_times'] + self.view_mode = params['view_mode'] + self.traffic = params['traffic'] + self.lidar_max_range = params['lidar_max_range'] + self.max_nearby_vehicles = params['max_nearby_vehicles'] + self.surrounding_vehicle_spawned_randomly = params['surrounding_vehicle_spawned_randomly'] + self.observation_space = spaces.Dict({ + 'lidar': spaces.Box(low=0.0, high=1.0, shape=(240,), dtype=np.float32), + 'ego_state': spaces.Box(low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32), + 'nearby_vehicles': spaces.Box(low=-np.inf, high=np.inf, shape=(self.max_nearby_vehicles, 6), dtype=np.float32), + 'waypoints': spaces.Box(low=-np.inf, high=np.inf, shape=(self.max_waypoints, 4), dtype=np.float32), + 'lane_info': spaces.Box(low=0.0, high=1.0, shape=(2,), dtype=np.float32), + }) + self.action_space = spaces.Box( + low=np.array([0.0, -1.0, 0.0], dtype=np.float32), + high=np.array([1.0, 1.0, 1.0], dtype=np.float32) + ) + + print('Connecting to Carla server...') + client = carla.Client('localhost', params['port']) + client.set_timeout(10.0) + self.world = client.load_world(params['town']) + self.world.set_weather(carla.WeatherParameters.ClearNoon) + print('Connection established!') + + # Get all predefined vehicle spawn points from the map + self.vehicle_spawn_points = list(self.world.get_map().get_spawn_points()) + # Prepare a list to hold spawn points for pedestrians (walkers) + self.walker_spawn_points = [] + # Randomly generate spawn points for the specified number of pedestrians + for i in range(self.number_of_walkers): + spawn_point = carla.Transform() # Create an empty transform object + # Try to get a random navigable location in the environment + loc = self.world.get_random_location_from_navigation() + # If a valid location is found, use it as a spawn point for a pedestrian + if loc is not None: + spawn_point.location = loc + self.walker_spawn_points.append(spawn_point) + + + self.ego_bp = self._create_vehicle_bluepprint(params['ego_vehicle_filter'], color='255,0,0') + + self.collision_hist = [] + self.collision_hist_l = 1 + self.collision_bp = self.world.get_blueprint_library().find('sensor.other.collision') + + self.lidar_data = None # Placeholder to store incoming LiDAR data + self.lidar_height = 0.8 # Height at which the LiDAR is mounted on the vehicle (in meters) + # Set the position of the LiDAR sensor using a transform (translation only in Z direction) + self.lidar_trans = carla.Transform(carla.Location(x=0.0, z=self.lidar_height)) + # Get the LiDAR blueprint from Carla's sensor library + self.lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast') + # Set LiDAR attributes + self.lidar_bp.set_attribute('channels', '1') # Use 1 channel to perform a flat 360ยฐ horizontal scan + self.lidar_bp.set_attribute('range', '50') # Maximum LiDAR range in meters + self.lidar_bp.set_attribute('rotation_frequency', '10') # How many full 360ยฐ rotations per second + self.lidar_bp.set_attribute('points_per_second', '10000') # Total number of points generated per second + self.lidar_bp.set_attribute('upper_fov', '0') # upper and lower FOV are both 0 for a flat horizontal scan + self.lidar_bp.set_attribute('lower_fov', '0') + + + self.settings = self.world.get_settings() # Get the current world settings + self.settings.fixed_delta_seconds = self.dt # Set the physics simulation step size (in seconds) + # This ensures consistent time intervals for simulation updates + + + self.reset_step = 0 + self.total_step = 0 + + def reset(self): + # Stop and destroy the collision sensor if it exists + if self.collision_sensor is not None: + try: + self.collision_sensor.stop() + self.collision_sensor.destroy() + except: + pass + self.collision_sensor = None + + # Stop and destroy the LiDAR sensor if it exists + if self.lidar_sensor is not None: + try: + self.lidar_sensor.stop() + self.lidar_sensor.destroy() + except: + pass + self.lidar_sensor = None + + # Reset collision and off-road status flags + self._is_collision = False + self._is_off_road = False + + self._set_synchronous_mode(False) # Switch back to asynchronous mode + self._clear_all_actors([ + 'sensor.other.collision', + 'sensor.lidar.ray_cast', + 'sensor.camera.rgb', + 'vehicle.*', + 'controller.ai.walker', + 'walker.*' + ]) # Remove all specified actors from the world + + # Spawn surrounding vehicles + random.shuffle(self.vehicle_spawn_points) + count = self.number_of_vehicles + self.spawned_vehicles = [] + self.used_spawn_points = [] + + if count > 0: + for spawn_point in self.vehicle_spawn_points: + vehicle = self._try_spawn_random_vehicle_at(spawn_point, number_of_wheels=[4]) + if vehicle: + self.spawned_vehicles.append(vehicle) # Record the spawned vehicle + self.used_spawn_points.append(spawn_point) # Mark spawn point as used + count -= 1 + if count <= 0: + break + # print(f"Surrounding vehicles number is {len(self.spawned_vehicles)}") + + # Spawn pedestrians + random.shuffle(self.walker_spawn_points) + count = self.number_of_walkers + + if count > 0: + for spawn_point in self.walker_spawn_points: + if self._try_spawn_random_walker_at(spawn_point): + count -= 1 + if count <= 0: + break + + # Try random spawn points until all pedestrians are spawned + while count > 0: + if self._try_spawn_random_walker_at(random.choice(self.walker_spawn_points)): + count -= 1 + + # Get actors' polygon list + # Calculate and collect the bounding polygons (e.g., four corners) of surrounding vehicles and pedestrians + self.vehicle_polygons = [] + vehicle_poly_dict = self._get_actor_polygons('vehicle.*') + self.vehicle_polygons.append(vehicle_poly_dict) + + self.walker_polygons = [] + walker_poly_dict = self._get_actor_polygons('walker.*') + self.walker_polygons.append(walker_poly_dict) + + # Spawn the ego vehicle + ego_spawn_times = 0 + while True: + if ego_spawn_times > self.max_ego_spawn_times: + self.reset() # If failed too many times, reset the environment + + # Select a spawn point for the ego vehicle by excluding locations used by nearby vehicles + available_spawn_points = [ + sp for sp in self.vehicle_spawn_points if sp not in self.used_spawn_points + ] + + if len(available_spawn_points) > 0: + transform = random.choice(available_spawn_points) # Choose a spawn point not used by nearby vehicles + else: + transform = random.choice(self.vehicle_spawn_points) # Fallback: use any spawn point + + # Try to spawn the ego vehicle at the selected location + if self._try_spawn_ego_vehicle_at(transform): + break # Successfully spawned the ego vehicle + else: + ego_spawn_times += 1 # Retry counter + time.sleep(0.1) # Small delay before retrying + + if self.traffic == 'off': + # Set all traffic lights to green and freeze them + for actor in self.world.get_actors().filter('traffic.traffic_light*'): + actor.set_state(carla.TrafficLightState.Green) + actor.freeze(True) + elif self.traffic == 'on': + # Allow traffic lights to work normally + for actor in self.world.get_actors().filter('traffic.traffic_light*'): + actor.freeze(False) + + # Add collision sensor + self.collision_sensor = self.world.spawn_actor( + self.collision_bp, + carla.Transform(), # Attach at the center of the ego vehicle (no offset) + attach_to=self.ego + ) + + # Start listening for collision events + self.collision_sensor.listen( + lambda event: get_collision_hist(event) # When a collision event happens, pass the event to get_collision_hist() + ) + + def get_collision_hist(event): + impulse = event.normal_impulse # Get the collision impulse (a 3D vector) + intensity = np.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) # Calculate collision intensity (vector norm) + self.collision_hist.append(intensity) # Record the collision intensity + if len(self.collision_hist) > self.collision_hist_l: + self.collision_hist.pop(0) # Keep only the latest collision records (FIFO) + + # Initialize collision history list + # Clear collision history after each episode because in gym-carla setup, + # a collision typically triggers episode termination and reset. + self.collision_hist = [] + + # Add lidar sensor + self.lidar_sensor = self.world.spawn_actor(self.lidar_bp, self.lidar_trans, attach_to=self.ego) + self.lidar_sensor.listen(lambda data: get_lidar_data(data)) + def get_lidar_data(data): + self.lidar_data = data + + # Update timesteps + self.time_step = 1 # Indicates a new episode has started + self.reset_step += 1 # Count how many resets have occurred + + # Enable autopilot for all surrounding vehicles + for vehicle in self.spawned_vehicles: + vehicle.set_autopilot() + + self._set_synchronous_mode(True) # Switch to synchronous mode for simulation + self.world.tick() # Advance the simulation by one tick + + return self._get_obs() # Return the initial observation after reset + + def step(self, action): + throttle = float(np.clip(action[0], 0.0, 1.0)) + steer = float(np.clip(action[1], -1.0, 1.0)) + brake = float(np.clip(action[2], 0.0, 1.0)) + + # Apply control + control = carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + self.ego.apply_control(control) + + self.world.tick() + + # Set spectator (camera) view + spectator = self.world.get_spectator() + transform = self.ego.get_transform() + if self.view_mode == 'top': + # Top-down view (bird's eye) + spectator.set_transform( + carla.Transform( + transform.location + carla.Location(z=40), + carla.Rotation(pitch=-90) + ) + ) + elif self.view_mode == 'follow': + # Follow view (behind and above the ego vehicle) + cam_location = transform.transform(carla.Location(x=-6.0, z=3.0)) # 6 meters behind, 3 meters above + cam_rotation = carla.Rotation(pitch=-10, yaw=transform.rotation.yaw, roll=0) + spectator.set_transform(carla.Transform(cam_location, cam_rotation)) + + # Update timesteps + self.time_step += 1 + self.total_step += 1 + + obs = self._get_obs() + done = self._terminal() + reward = self._get_reward(obs, done) + cost = self._get_cost(obs) + + # state information + info = { + 'is_collision': self._is_collision, + 'is_off_road': self._is_off_road + } + return (obs, reward, cost, done, info) + + def _create_vehicle_bluepprint(self, actor_filter, color=None, number_of_wheels=[4]): + """Create a vehicle blueprint based on the given filter and wheel number. + + Args: + actor_filter (str): Filter string to select vehicle types, e.g., 'vehicle.lincoln*' + ('*' matches a series of models). + color (str, optional): Desired vehicle color. Randomly chosen if None. + number_of_wheels (list): A list of acceptable wheel numbers (default is [4]). + + Returns: + bp (carla.ActorBlueprint): A randomly selected blueprint matching the criteria. + """ + # Get all blueprints matching the actor filter + blueprints = self.world.get_blueprint_library().filter(actor_filter) + blueprint_library = [] + + # Further filter blueprints based on the number of wheels + # Keeping number_of_wheels as a list makes it flexible to match multiple types (e.g., cars, trucks) + for nw in number_of_wheels: + blueprint_library += [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == nw] + + # Randomly select one blueprint from the filtered list + bp = random.choice(blueprint_library) + + # Set the vehicle color + if bp.has_attribute('color'): + if not color: + color = random.choice(bp.get_attribute('color').recommended_values) + bp.set_attribute('color', color) + + return bp + + def _set_synchronous_mode(self, synchronous=True): + + """Enable or disable synchronous mode for the simulation. + Args: + synchronous (bool): + True to enable synchronous mode (server waits for client each frame), + False to disable and run in asynchronous mode (default is True). + """ + self.settings.synchronous_mode = synchronous # Set the synchronous mode + self.world.apply_settings(self.settings) # Apply the updated settings to the world + + def _try_spawn_random_vehicle_at(self, transform, number_of_wheels=[4]): + """Try to spawn a surrounding vehicle at a specific transform. + + Args: + transform (carla.Transform): Location and orientation where the vehicle should be spawned. + number_of_wheels (list): Acceptable number(s) of wheels for the vehicle blueprint. + random_vehicle (bool): + False to use Tesla Model 3 with a blue color, + True to randomly select a vehicle model and color (default). + + Returns: + carla.Actor or None: Spawned vehicle actor if successful, otherwise None. + """ + if self.surrounding_vehicle_spawned_randomly: + # Randomly choose any vehicle blueprint + blueprint = self._create_vehicle_bluepprint('vehicle.*', number_of_wheels=number_of_wheels) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + else: + # Fixed: Tesla Model 3 with blue color + blueprint = self._create_vehicle_bluepprint('vehicle.tesla.model3', color='0,0,255', number_of_wheels=number_of_wheels) + + blueprint.set_attribute('role_name', 'autopilot') # Set the vehicle to autopilot mode + + # Try to spawn the vehicle + vehicle = self.world.try_spawn_actor(blueprint, transform) + + return vehicle if vehicle is not None else None + + def _try_spawn_random_walker_at(self, transform): + """Try to spawn a walker at a specific transform with a random blueprint. + + Args: + transform (carla.Transform): Location and orientation where the walker should be spawned. + + Returns: + Bool: True if spawn is successful, False otherwise. + """ + # Randomly select a walker blueprint + walker_bp = random.choice(self.world.get_blueprint_library().filter('walker.*')) + + # Make the walker vulnerable (can be affected by collisions) + if walker_bp.has_attribute('is_invincible'): + walker_bp.set_attribute('is_invincible', 'false') + + # Try to spawn the walker actor + walker_actor = self.world.try_spawn_actor(walker_bp, transform) + + if walker_actor is not None: + # Spawn a controller for the walker + walker_controller_bp = self.world.get_blueprint_library().find('controller.ai.walker') + walker_controller_actor = self.world.spawn_actor(walker_controller_bp, carla.Transform(), walker_actor) + + # Start the controller to control the walker + walker_controller_actor.start() + + # Move the walker to a random location + walker_controller_actor.go_to_location(self.world.get_random_location_from_navigation()) + + # Set a random walking speed between 1 m/s and 2 m/s (default is 1.4 m/s) + walker_controller_actor.set_max_speed(1 + random.random()) + + return True # Spawn and initialization successful + + return False # Failed to spawn + + def _try_spawn_ego_vehicle_at(self, transform): + """Try to spawn the ego vehicle at a specific transform. + + Args: + transform (carla.Transform): Target location and orientation. + + Returns: + Bool: True if spawn is successful, False otherwise. + """ + vehicle = None + overlap = False + + # Check if ego position overlaps with surrounding vehicles + for idx, poly in self.vehicle_polygons[-1].items(): # Use .items() to iterate over dict keys and values + poly_center = np.mean(poly, axis=0) + ego_center = np.array([transform.location.x, transform.location.y]) + dis = np.linalg.norm(poly_center - ego_center) + + if dis > 8: + continue + else: + overlap = True + break + + # If no overlap, try to spawn the ego vehicle + if not overlap: + vehicle = self.world.try_spawn_actor(self.ego_bp, transform) + + if vehicle is not None: + self.ego = vehicle + return True + + return False + + def _get_actor_polygons(self, filt): + """Get the bounding box polygon of actors. + + Args: + filt: the filter indicating what type of actors we'll look at. + + Returns: + actor_poly_dict: a dictionary containing the bounding boxes of specific actors. + """ + actor_poly_dict = {} + for actor in self.world.get_actors().filter(filt): + # Get all actors in the current world that meet the filt condition, such as vehicle.* or walker.* + # Note that self.world.get_actors() retrieves all objects in the current simulation environment (vehicles, pedestrians, traffic lights, etc.). + + # Get x, y and yaw of the actor + trans = actor.get_transform() + # Get the actor's global position (location) and heading angle (rotation). + + x = trans.location.x + # x, y are the actor's global coordinates. + + y = trans.location.y + yaw = trans.rotation.yaw / 180 * np.pi + # yaw is the heading angle, whose unit is degrees, needs to be converted to radians (multiply by pi/180) to facilitate subsequent matrix calculations. + + # Get length and width + bb = actor.bounding_box + # Get the "half-length" boundary. + + l = bb.extent.x + # "Half-length" in the x-direction (the distance from the center to the edge). + + w = bb.extent.y + # "Half-width" in the y-direction (the distance from the center to the edge). + + # Get bounding box polygon in the actor's local coordinate + # Take the vehicle center as the origin, build a local coordinate system, and list four corner points: + # (l, w): front right corner, (l, -w): rear right corner, (-l, -w): rear left corner, (-l, w): front left corner + poly_local = np.array([ + [l, w], [l, -w], [-l, -w], [-l, w] + ]).transpose() + # Transpose() here is to facilitate subsequent matrix operations, + # changing the matrix from (4,2) to (2,4) format. + + # Get rotation matrix to transform to global coordinate + # This is a standard 2D rotation matrix: used to transform points from the local coordinate system to the global coordinate system. + # Rotation matrix R = [cosฮธ -sinฮธ] + # [sinฮธ cosฮธ] + R = np.array([ + [np.cos(yaw), -np.sin(yaw)], + [np.sin(yaw), np.cos(yaw)] + ]) + + # Get global bounding box polygon + poly = np.matmul(R, poly_local).transpose() + np.repeat([[x, y]], 4, axis=0) + # np.matmul(R, poly_local): + # Transform the four corners (in the local coordinate system) into the global direction through the rotation matrix. + # After .transpose(), it becomes (4,2) format (one point per row). + # + np.repeat([[x,y]],4,axis=0): + # Add the global position offset of the vehicle/pedestrian to each point + # to obtain the final polygon coordinates in the global coordinate system. + + actor_poly_dict[actor.id] = poly + # Store the calculated poly (a 4ร—2 array, four corner points in global coordinates) + # with actor.id as the key into actor_poly_dict. + # After returning, the entire dictionary structure: + # { + # actor_id_1: np.array([[x1,y1],[x2,y2],[x3,y3],[x4,y4]]), + # actor_id_2: np.array([[x1,y1],[x2,y2],[x3,y3],[x4,y4]]), + # ... + # } + + return actor_poly_dict + + def _get_obs(self): + obs = {} + +# ========================== LIDAR feature extraction (240 dimensions) ========================== + max_range = self.lidar_max_range # Set a maximum perception distance + lidar_features = np.full((240,), max_range, dtype=np.float32) # Initialize all values to the maximum distance + + # Get ego pose + ego_transform = self.ego.get_transform() + ego_x = ego_transform.location.x + ego_y = ego_transform.location.y + ego_yaw = np.deg2rad(ego_transform.rotation.yaw) + + # Traverse all point clouds + for detection in self.lidar_data: + x = detection.point.x + y = detection.point.y + + # Rotate back to ego vehicle heading direction (make ego vehicle heading as 0 degrees) + local_x = np.cos(-ego_yaw) * x - np.sin(-ego_yaw) * y + local_y = np.sin(-ego_yaw) * x + np.cos(-ego_yaw) * y + + distance = np.sqrt(local_x**2 + local_y**2) + angle = np.arctan2(local_y, local_x) # Range [-ฯ€, ฯ€] + angle_deg = (np.degrees(angle) + 360) % 360 # Map to [0, 360) + index = int(angle_deg // 1.5) # Each angular bin has a width of 1.5 degrees, 240 bins in total + + if index < 240: + lidar_features[index] = min(lidar_features[index], distance) + + # Normalize to [0, 1] + lidar_features /= max_range + + # Store into observation + obs['lidar'] = lidar_features + +# ========================== Ego vehicle state extraction ======================================= + velocity = self.ego.get_velocity() + speed = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + angular_velocity = self.ego.get_angular_velocity() + acceleration = self.ego.get_acceleration() + + front_vehicle_distance = 0.0 + relative_speed = 0.0 + + min_front_distance = 20.0 # Search range threshold + vehicle_list = self.world.get_actors().filter('vehicle.*') + + for vehicle in vehicle_list: + if vehicle.id == self.ego.id: + continue + + transform = vehicle.get_transform() + rel_x = transform.location.x - ego_x + rel_y = transform.location.y - ego_y + + local_x = np.cos(-ego_yaw) * rel_x - np.sin(-ego_yaw) * rel_y + local_y = np.sin(-ego_yaw) * rel_x + np.cos(-ego_yaw) * rel_y + + if 0 < local_x < min_front_distance and abs(local_y) < 2.5: + d = np.sqrt(local_x**2 + local_y**2) + if front_vehicle_distance == 0.0 or d < front_vehicle_distance: + front_vehicle_distance = d + front_speed = vehicle.get_velocity() + front_speed_mag = np.sqrt(front_speed.x**2 + front_speed.y**2 + front_speed.z**2) + relative_speed = speed - front_speed_mag + + ego_state = np.array([ + ego_x, + ego_y, + ego_yaw, + speed, + angular_velocity.z, + acceleration.x, + acceleration.y, + front_vehicle_distance, + relative_speed + ], dtype=np.float32) + + obs['ego_state'] = ego_state + +# ================ Nearby vehicles state extraction (up to 5 vehicles, within perception range) =============== + max_vehicles = self.max_nearby_vehicles + perception_range = self.lidar_max_range + vehicle_list = self.world.get_actors().filter('vehicle.*') + + vehicle_data = [] + for vehicle in vehicle_list: + if vehicle.id == self.ego.id: + continue # Skip the ego vehicle itself + + transform = vehicle.get_transform() + x = transform.location.x + y = transform.location.y + yaw = np.deg2rad(transform.rotation.yaw) + + rel_x = x - ego_x + rel_y = y - ego_y + + distance = np.sqrt(rel_x**2 + rel_y**2) + if distance > perception_range: + continue # Ignore vehicles outside the perception range + + # Transform to ego-centric local coordinates + local_x = np.cos(-ego_yaw) * rel_x - np.sin(-ego_yaw) * rel_y + local_y = np.sin(-ego_yaw) * rel_x + np.cos(-ego_yaw) * rel_y + + v = vehicle.get_velocity() + speed = np.sqrt(v.x**2 + v.y**2 + v.z**2) + + vehicle_data.append((distance, [local_x, local_y, yaw - ego_yaw, speed])) + + # Sort vehicles by distance and select the nearest max_vehicles + vehicle_data.sort(key=lambda x: x[0]) + nearby_vehicles = [data for _, data in vehicle_data[:max_vehicles]] + + # Pad with zeros if fewer than max_vehicles are detected + while len(nearby_vehicles) < max_vehicles: + nearby_vehicles.append([0.0, 0.0, 0.0, 0.0]) + + obs['nearby_vehicles'] = np.array(nearby_vehicles, dtype=np.float32).flatten() + +# ========================== Current reference waypoints (up to N waypoints) ========================== + max_waypoints = self.max_waypoints + world_map = self.world.get_map() + waypoint = world_map.get_waypoint(self.ego.get_location()) + waypoints_array = np.zeros((max_waypoints, 3), dtype=np.float32) + + for i in range(max_waypoints): + if waypoint is None: + break + + loc = waypoint.transform.location + yaw = waypoint.transform.rotation.yaw + + # Transform waypoint location into ego-centric local coordinates + local_x = np.cos(-ego_yaw) * (loc.x - ego_x) - np.sin(-ego_yaw) * (loc.y - ego_y) + local_y = np.sin(-ego_yaw) * (loc.x - ego_x) + np.cos(-ego_yaw) * (loc.y - ego_y) + yaw_relative = np.deg2rad(yaw) - ego_yaw # Relative heading + + waypoints_array[i] = [local_x, local_y, yaw_relative] + + # Move to the next waypoint 2.0 meters ahead + next_waypoints = waypoint.next(2.0) + waypoint = next_waypoints[0] if next_waypoints else None + + obs['waypoints'] = waypoints_array.flatten() + +# ============================= Lane boundary information ========================================= + waypoint_center = world_map.get_waypoint( + self.ego.get_location(), project_to_road=True, lane_type=carla.LaneType.Driving + ) + + if waypoint_center is None: + # If no valid driving lane is found + obs['lane_info'] = np.array([0.0, 0.0], dtype=np.float32) + else: + lane_width = waypoint_center.lane_width + ego_location = self.ego.get_location() + center_location = waypoint_center.transform.location + + # Calculate lateral offset between ego position and lane centerline + lateral_offset = np.linalg.norm( + np.array([ + ego_location.x - center_location.x, + ego_location.y - center_location.y + ]) + ) + + obs['lane_info'] = np.array([lane_width, lateral_offset], dtype=np.float32) + +# =============================== Visualize current reference waypoints =============================== + if self.visualize_waypoints: + for i in range(max_waypoints): + wx, wy, _ = waypoints_array[i] + + # Transform from ego-centric local coordinates to global coordinates + gx = np.cos(ego_yaw) * wx - np.sin(ego_yaw) * wy + ego_x + gy = np.sin(ego_yaw) * wx + np.cos(ego_yaw) * wy + ego_y + + self.world.debug.draw_point( + carla.Location(x=gx, y=gy, z=ego_transform.location.z + 1.0), + size=0.1, + life_time=0.5, + color=carla.Color(0, 255, 0) # Green points + ) + + return obs + + def _get_reward(self, obs, done): + reward = 0.0 + + # 1. Forward driving reward (within speed limit and along lane direction) + speed = obs['ego_state'][3] + if speed <= self.desired_speed: + reward += 1.0 * speed + else: + reward += -1.0 * (speed - self.desired_speed) + + # 2. Lane deviation penalty (penalize offset from lane center) + lane_width, lateral_offset = obs['lane_info'] + reward += -1.0 * lateral_offset + + # 3. Smooth driving penalty (lateral acceleration penalty) + a_lat = obs['ego_state'][6] + reward += -0.5 * abs(a_lat) + + # 4. Stationary penalty (if no vehicle ahead but ego is barely moving) + front_distance = obs['ego_state'][7] + if front_distance > 10.0 and speed < 0.1: + reward += -1.0 + + # 5. Collision penalty + if self._is_collision: + reward += -100.0 + + # 6. Off-road penalty + if self._is_off_road: + reward += -100.0 + + # # 7. Sparse terminal reward (for safely reaching the goal) + # if done: + # if not self._is_collision and not self._is_off_road: + # reward += 200.0 + + return reward + + def _get_cost(self, obs): + """ + Calculate the constraint cost for safe reinforcement learning. + + This cost is only used in safe RL settings and does not affect the reward function. + It penalizes collisions, off-road events, and overspeeding behavior. + + Args: + obs: The current observation dictionary. + + Returns: + cost (float): The accumulated constraint cost. + """ + cost = 0.0 + + # 1. Collision cost + if self._is_collision: + cost += 20.0 + + # 2. Off-road cost + if self._is_off_road: + cost += 20.0 + + # 3. Overspeed cost + speed = obs['ego_state'][3] + if speed > self.desired_speed: + cost += (speed - self.desired_speed) / self.desired_speed # Cost proportional to overspeed percentage + + return cost + + def _terminal(self): + ego_transform = self.ego.get_transform() + ego_x = ego_transform.location.x + ego_y = ego_transform.location.y + + # 1. Collision termination + if len(self.collision_hist) > 0: + self._is_collision = True + print('Collision occurred') + return True + + # 2. Exceeding maximum allowed timesteps + if self.time_step > self.max_time_episode: + print('Exceeded maximum timesteps') + return True + + # # 3. Goal reaching termination (optional) + # if self.dests is not None: + # for dest in self.dests: + # if np.sqrt((ego_x - dest[0])**2 + (ego_y - dest[1])**2) < 4: + # return True + + # 4. Check if the current lane is a drivable lane + waypoint = self.world.get_map().get_waypoint( + self.ego.get_location(), + project_to_road=True, + lane_type=carla.LaneType.Driving + ) + if waypoint is None: + self._is_off_road = True + print('Non-drivable lane detected') + return True + + # 5. Check if the vehicle's heading deviates too much from lane direction (> ยฑ90ยฐ) + ego_yaw = self.ego.get_transform().rotation.yaw + lane_yaw = waypoint.transform.rotation.yaw + yaw_diff = np.deg2rad(ego_yaw - lane_yaw) + yaw_diff = np.arctan2(np.sin(yaw_diff), np.cos(yaw_diff)) # Normalize to [-ฯ€, ฯ€] + if not waypoint.is_intersection: + if abs(yaw_diff) > np.pi / 2: # More than 90 degrees deviation (wrong-way driving) + self._is_off_road = True + print('Wrong-way driving detected') + return True + + # 6. Deviation too far from lane center + lane_width, lateral_offset = self._get_obs()['lane_info'] + if not waypoint.is_intersection: + if lateral_offset > lane_width / 2 + 1.0: + self._is_off_road = True + print('Deviated from lane') + return True + + return False + + def _clear_all_actors(self, actor_filters): + """Clear (destroy) all actors matching the given filter patterns. + + Args: + actor_filters (list): A list of filter strings, e.g., ['vehicle.*', 'walker.*', 'sensor.*']. + """ + for actor_filter in actor_filters: + for actor in self.world.get_actors().filter(actor_filter): + try: + # If the actor is a sensor, stop it before destroying + if 'sensor' in actor.type_id: + actor.stop() + actor.destroy() + except: + pass # Ignore any errors during destruction + + + + + + + + diff --git a/easycarla_demo.py b/easycarla_demo.py new file mode 100644 index 0000000..91bf7ed --- /dev/null +++ b/easycarla_demo.py @@ -0,0 +1,81 @@ +""" +Author: SilverWings +GitHub: https://github.com/silverwingsbot + +This script provides a minimal demo to interact with the EasyCarla-RL environment. +It follows the standard Gym interface (reset, step) and demonstrates basic environment usage. + +""" + +import gym +import easycarla +import carla +import random +import numpy as np + +# Configure environment parameters +params = { + 'number_of_vehicles': 100, + 'number_of_walkers': 0, + 'dt': 0.1, # time interval between two frames + 'ego_vehicle_filter': 'vehicle.tesla.model3', # filter for defining ego vehicle + 'surrounding_vehicle_spawned_randomly': True, # Whether surrounding vehicles are spawned randomly (True) or set manually (False) + 'port': 2000, # connection port + 'town': 'Town03', # which town to simulate + 'max_time_episode': 1000, # maximum timesteps per episode + 'max_waypoints': 12, # maximum number of waypoints + 'visualize_waypoints': True, # Whether to visualize waypoints (default: True) + 'desired_speed': 8, # desired speed (m/s) + 'max_ego_spawn_times': 200, # maximum times to spawn ego vehicle + 'view_mode' : 'top', # 'top' for bird's-eye view, 'follow' for third-person view + 'traffic': 'off', # 'on' for normal traffic lights, 'off' for always green and frozen + 'lidar_max_range': 50.0, # Maximum LIDAR perception range (meters) + 'max_nearby_vehicles': 5, # Maximum number of nearby vehicles to observe +} + +# Create the environment +env = gym.make('carla-v0', params=params) +obs = env.reset() + +# Define a simple action policy +def get_action(env, obs): + """Randomly choose either a simple manual action or an autopilot action.""" + p = random.random() + if p < 0.5: + # Use autopilot (Expert mode) + env.ego.set_autopilot(True) + control = env.ego.get_control() + action = [control.throttle, control.steer, control.brake] + else: + # Use random action (Novice mode) + env.ego.set_autopilot(False) + throttle = random.uniform(0.0, 1.0) + steer = random.uniform(-0.6, 0.6) + brake = random.uniform(0.0, 0.3) + action = [throttle, steer, brake] + return action + +# Interact with the environment +for episode in range(5): # Run 5 episodes + obs = env.reset() + done = False + total_reward = 0 + + while not done: + action = get_action(env, obs) + next_obs, reward, cost, done, info = env.step(action) + + print(f"Step: {env.time_step}, Reward: {reward:.2f}, Cost: {cost:.2f}, Done: {done}") + + obs = next_obs + total_reward += reward + + print(f"Episode {episode} finished. Total reward: {total_reward:.2f}") + +env.close() + + + + + + diff --git a/example/agents/__init__.py b/example/agents/__init__.py new file mode 100644 index 0000000..139597f --- /dev/null +++ b/example/agents/__init__.py @@ -0,0 +1,2 @@ + + diff --git a/example/agents/bc_diffusion.py b/example/agents/bc_diffusion.py new file mode 100644 index 0000000..22d99ac --- /dev/null +++ b/example/agents/bc_diffusion.py @@ -0,0 +1,77 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import copy +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +from utils.logger import logger + +from agents.diffusion import Diffusion +from agents.model import MLP + + +class Diffusion_BC(object): + def __init__(self, + state_dim, + action_dim, + max_action, + device, + discount, + tau, + beta_schedule='linear', + n_timesteps=100, + lr=2e-4, + ): + + self.model = MLP(state_dim=state_dim, action_dim=action_dim, device=device) + self.actor = Diffusion(state_dim=state_dim, action_dim=action_dim, model=self.model, max_action=max_action, + beta_schedule=beta_schedule, n_timesteps=n_timesteps, + ).to(device) + self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=lr) + + self.max_action = max_action + self.action_dim = action_dim + self.discount = discount + self.tau = tau + self.device = device + + def train(self, replay_buffer, iterations, batch_size=100, log_writer=None): + + metric = {'bc_loss': [], 'ql_loss': [], 'actor_loss': [], 'critic_loss': []} + for _ in range(iterations): + # Sample replay buffer / batch + state, action, next_state, reward, not_done = replay_buffer.sample(batch_size) + + loss = self.actor.loss(action, state) + + self.actor_optimizer.zero_grad() + loss.backward() + self.actor_optimizer.step() + + metric['actor_loss'].append(0.) + metric['bc_loss'].append(loss.item()) + metric['ql_loss'].append(0.) + metric['critic_loss'].append(0.) + + return metric + + def sample_action(self, state): + state = torch.FloatTensor(state.reshape(1, -1)).to(self.device) + with torch.no_grad(): + action = self.actor.sample(state) + return action.cpu().data.numpy().flatten() + + def save_model(self, dir, id=None): + if id is not None: + torch.save(self.actor.state_dict(), f'{dir}/actor_{id}.pth') + else: + torch.save(self.actor.state_dict(), f'{dir}/actor.pth') + + def load_model(self, dir, id=None): + if id is not None: + self.actor.load_state_dict(torch.load(f'{dir}/actor_{id}.pth')) + else: + self.actor.load_state_dict(torch.load(f'{dir}/actor.pth')) + diff --git a/example/agents/diffusion.py b/example/agents/diffusion.py new file mode 100644 index 0000000..a8bed70 --- /dev/null +++ b/example/agents/diffusion.py @@ -0,0 +1,183 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import copy +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F + + +from agents.helpers import (cosine_beta_schedule, + linear_beta_schedule, + vp_beta_schedule, + extract, + Losses) +from utils.utils import Progress, Silent + + +class Diffusion(nn.Module): + def __init__(self, state_dim, action_dim, model, max_action, + beta_schedule='linear', n_timesteps=100, + loss_type='l2', clip_denoised=True, predict_epsilon=True): + super(Diffusion, self).__init__() + + self.state_dim = state_dim + self.action_dim = action_dim + self.max_action = max_action + self.model = model + + if beta_schedule == 'linear': + betas = linear_beta_schedule(n_timesteps) + elif beta_schedule == 'cosine': + betas = cosine_beta_schedule(n_timesteps) + elif beta_schedule == 'vp': + betas = vp_beta_schedule(n_timesteps) + + alphas = 1. - betas + alphas_cumprod = torch.cumprod(alphas, axis=0) + alphas_cumprod_prev = torch.cat([torch.ones(1), alphas_cumprod[:-1]]) + + self.n_timesteps = int(n_timesteps) + self.clip_denoised = clip_denoised + self.predict_epsilon = predict_epsilon + + self.register_buffer('betas', betas) + self.register_buffer('alphas_cumprod', alphas_cumprod) + self.register_buffer('alphas_cumprod_prev', alphas_cumprod_prev) + + # calculations for diffusion q(x_t | x_{t-1}) and others + self.register_buffer('sqrt_alphas_cumprod', torch.sqrt(alphas_cumprod)) + self.register_buffer('sqrt_one_minus_alphas_cumprod', torch.sqrt(1. - alphas_cumprod)) + self.register_buffer('log_one_minus_alphas_cumprod', torch.log(1. - alphas_cumprod)) + self.register_buffer('sqrt_recip_alphas_cumprod', torch.sqrt(1. / alphas_cumprod)) + self.register_buffer('sqrt_recipm1_alphas_cumprod', torch.sqrt(1. / alphas_cumprod - 1)) + + # calculations for posterior q(x_{t-1} | x_t, x_0) + posterior_variance = betas * (1. - alphas_cumprod_prev) / (1. - alphas_cumprod) + self.register_buffer('posterior_variance', posterior_variance) + + ## log calculation clipped because the posterior variance + ## is 0 at the beginning of the diffusion chain + self.register_buffer('posterior_log_variance_clipped', + torch.log(torch.clamp(posterior_variance, min=1e-20))) + self.register_buffer('posterior_mean_coef1', + betas * np.sqrt(alphas_cumprod_prev) / (1. - alphas_cumprod)) + self.register_buffer('posterior_mean_coef2', + (1. - alphas_cumprod_prev) * np.sqrt(alphas) / (1. - alphas_cumprod)) + + self.loss_fn = Losses[loss_type]() + + # ------------------------------------------ sampling ------------------------------------------# + + def predict_start_from_noise(self, x_t, t, noise): + ''' + if self.predict_epsilon, model output is (scaled) noise; + otherwise, model predicts x0 directly + ''' + if self.predict_epsilon: + return ( + extract(self.sqrt_recip_alphas_cumprod, t, x_t.shape) * x_t - + extract(self.sqrt_recipm1_alphas_cumprod, t, x_t.shape) * noise + ) + else: + return noise + + def q_posterior(self, x_start, x_t, t): + posterior_mean = ( + extract(self.posterior_mean_coef1, t, x_t.shape) * x_start + + extract(self.posterior_mean_coef2, t, x_t.shape) * x_t + ) + posterior_variance = extract(self.posterior_variance, t, x_t.shape) + posterior_log_variance_clipped = extract(self.posterior_log_variance_clipped, t, x_t.shape) + return posterior_mean, posterior_variance, posterior_log_variance_clipped + + def p_mean_variance(self, x, t, s): + x_recon = self.predict_start_from_noise(x, t=t, noise=self.model(x, t, s)) + + if self.clip_denoised: + x_recon.clamp_(-self.max_action, self.max_action) + else: + assert RuntimeError() + + model_mean, posterior_variance, posterior_log_variance = self.q_posterior(x_start=x_recon, x_t=x, t=t) + return model_mean, posterior_variance, posterior_log_variance + + # @torch.no_grad() + def p_sample(self, x, t, s): + b, *_, device = *x.shape, x.device + model_mean, _, model_log_variance = self.p_mean_variance(x=x, t=t, s=s) + noise = torch.randn_like(x) + # no noise when t == 0 + nonzero_mask = (1 - (t == 0).float()).reshape(b, *((1,) * (len(x.shape) - 1))) + return model_mean + nonzero_mask * (0.5 * model_log_variance).exp() * noise + + # @torch.no_grad() + def p_sample_loop(self, state, shape, verbose=False, return_diffusion=False): + device = self.betas.device + + batch_size = shape[0] + x = torch.randn(shape, device=device) + + if return_diffusion: diffusion = [x] + + progress = Progress(self.n_timesteps) if verbose else Silent() + for i in reversed(range(0, self.n_timesteps)): + timesteps = torch.full((batch_size,), i, device=device, dtype=torch.long) + x = self.p_sample(x, timesteps, state) + + progress.update({'t': i}) + + if return_diffusion: diffusion.append(x) + + progress.close() + + if return_diffusion: + return x, torch.stack(diffusion, dim=1) + else: + return x + + # @torch.no_grad() + def sample(self, state, *args, **kwargs): + batch_size = state.shape[0] + shape = (batch_size, self.action_dim) + action = self.p_sample_loop(state, shape, *args, **kwargs) + return action.clamp_(-self.max_action, self.max_action) + + # ------------------------------------------ training ------------------------------------------# + + def q_sample(self, x_start, t, noise=None): + if noise is None: + noise = torch.randn_like(x_start) + + sample = ( + extract(self.sqrt_alphas_cumprod, t, x_start.shape) * x_start + + extract(self.sqrt_one_minus_alphas_cumprod, t, x_start.shape) * noise + ) + + return sample + + def p_losses(self, x_start, state, t, weights=1.0): + noise = torch.randn_like(x_start) + + x_noisy = self.q_sample(x_start=x_start, t=t, noise=noise) + + x_recon = self.model(x_noisy, t, state) + + assert noise.shape == x_recon.shape + + if self.predict_epsilon: + loss = self.loss_fn(x_recon, noise, weights) + else: + loss = self.loss_fn(x_recon, x_start, weights) + + return loss + + def loss(self, x, state, weights=1.0): + batch_size = len(x) + t = torch.randint(0, self.n_timesteps, (batch_size,), device=x.device).long() + return self.p_losses(x, state, t, weights) + + def forward(self, state, *args, **kwargs): + return self.sample(state, *args, **kwargs) + diff --git a/example/agents/helpers.py b/example/agents/helpers.py new file mode 100644 index 0000000..03e1569 --- /dev/null +++ b/example/agents/helpers.py @@ -0,0 +1,116 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import math +import time +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F + +class SinusoidalPosEmb(nn.Module): + def __init__(self, dim): + super().__init__() + self.dim = dim + + def forward(self, x): + device = x.device + half_dim = self.dim // 2 + emb = math.log(10000) / (half_dim - 1) + emb = torch.exp(torch.arange(half_dim, device=device) * -emb) + emb = x[:, None] * emb[None, :] + emb = torch.cat((emb.sin(), emb.cos()), dim=-1) + return emb + +#-----------------------------------------------------------------------------# +#---------------------------------- sampling ---------------------------------# +#-----------------------------------------------------------------------------# + + +def extract(a, t, x_shape): + b, *_ = t.shape + out = a.gather(-1, t) + return out.reshape(b, *((1,) * (len(x_shape) - 1))) + + +def cosine_beta_schedule(timesteps, s=0.008, dtype=torch.float32): + """ + cosine schedule + as proposed in https://openreview.net/forum?id=-NEXDKk8gZ + """ + steps = timesteps + 1 + x = np.linspace(0, steps, steps) + alphas_cumprod = np.cos(((x / steps) + s) / (1 + s) * np.pi * 0.5) ** 2 + alphas_cumprod = alphas_cumprod / alphas_cumprod[0] + betas = 1 - (alphas_cumprod[1:] / alphas_cumprod[:-1]) + betas_clipped = np.clip(betas, a_min=0, a_max=0.999) + return torch.tensor(betas_clipped, dtype=dtype) + + +def linear_beta_schedule(timesteps, beta_start=1e-4, beta_end=2e-2, dtype=torch.float32): + betas = np.linspace( + beta_start, beta_end, timesteps + ) + return torch.tensor(betas, dtype=dtype) + + +def vp_beta_schedule(timesteps, dtype=torch.float32): + t = np.arange(1, timesteps + 1) + T = timesteps + b_max = 10. + b_min = 0.1 + alpha = np.exp(-b_min / T - 0.5 * (b_max - b_min) * (2 * t - 1) / T ** 2) + betas = 1 - alpha + return torch.tensor(betas, dtype=dtype) + +#-----------------------------------------------------------------------------# +#---------------------------------- losses -----------------------------------# +#-----------------------------------------------------------------------------# + +class WeightedLoss(nn.Module): + + def __init__(self): + super().__init__() + + def forward(self, pred, targ, weights=1.0): + ''' + pred, targ : tensor [ batch_size x action_dim ] + ''' + loss = self._loss(pred, targ) + weighted_loss = (loss * weights).mean() + return weighted_loss + +class WeightedL1(WeightedLoss): + + def _loss(self, pred, targ): + return torch.abs(pred - targ) + +class WeightedL2(WeightedLoss): + + def _loss(self, pred, targ): + return F.mse_loss(pred, targ, reduction='none') + + +Losses = { + 'l1': WeightedL1, + 'l2': WeightedL2, +} + + +class EMA(): + ''' + empirical moving average + ''' + def __init__(self, beta): + super().__init__() + self.beta = beta + + def update_model_average(self, ma_model, current_model): + for current_params, ma_params in zip(current_model.parameters(), ma_model.parameters()): + old_weight, up_weight = ma_params.data, current_params.data + ma_params.data = self.update_average(old_weight, up_weight) + + def update_average(self, old, new): + if old is None: + return new + return old * self.beta + (1 - self.beta) * new diff --git a/example/agents/model.py b/example/agents/model.py new file mode 100644 index 0000000..986c66d --- /dev/null +++ b/example/agents/model.py @@ -0,0 +1,50 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F + +from agents.helpers import SinusoidalPosEmb + + +class MLP(nn.Module): + """ + MLP Model + """ + def __init__(self, + state_dim, + action_dim, + device, + t_dim=16): + + super(MLP, self).__init__() + self.device = device + + self.time_mlp = nn.Sequential( + SinusoidalPosEmb(t_dim), + nn.Linear(t_dim, t_dim * 2), + nn.Mish(), + nn.Linear(t_dim * 2, t_dim), + ) + + input_dim = state_dim + action_dim + t_dim + self.mid_layer = nn.Sequential(nn.Linear(input_dim, 256), + nn.Mish(), + nn.Linear(256, 256), + nn.Mish(), + nn.Linear(256, 256), + nn.Mish()) + + self.final_layer = nn.Linear(256, action_dim) + + def forward(self, x, time, state): + + t = self.time_mlp(time) + x = torch.cat([x, t, state], dim=1) + x = self.mid_layer(x) + + return self.final_layer(x) + + diff --git a/example/agents/ql_diffusion.py b/example/agents/ql_diffusion.py new file mode 100644 index 0000000..bd42507 --- /dev/null +++ b/example/agents/ql_diffusion.py @@ -0,0 +1,212 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import copy +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +from torch.optim.lr_scheduler import CosineAnnealingLR +from utils.logger import logger + +from agents.diffusion import Diffusion +from agents.model import MLP +from agents.helpers import EMA + + +class Critic(nn.Module): + def __init__(self, state_dim, action_dim, hidden_dim=256): + super(Critic, self).__init__() + self.q1_model = nn.Sequential(nn.Linear(state_dim + action_dim, hidden_dim), + nn.Mish(), + nn.Linear(hidden_dim, hidden_dim), + nn.Mish(), + nn.Linear(hidden_dim, hidden_dim), + nn.Mish(), + nn.Linear(hidden_dim, 1)) + + self.q2_model = nn.Sequential(nn.Linear(state_dim + action_dim, hidden_dim), + nn.Mish(), + nn.Linear(hidden_dim, hidden_dim), + nn.Mish(), + nn.Linear(hidden_dim, hidden_dim), + nn.Mish(), + nn.Linear(hidden_dim, 1)) + + def forward(self, state, action): + x = torch.cat([state, action], dim=-1) + return self.q1_model(x), self.q2_model(x) + + def q1(self, state, action): + x = torch.cat([state, action], dim=-1) + return self.q1_model(x) + + def q_min(self, state, action): + q1, q2 = self.forward(state, action) + return torch.min(q1, q2) + + +class Diffusion_QL(object): + def __init__(self, + state_dim, + action_dim, + max_action, + device, + discount, + tau, + max_q_backup=False, + eta=1.0, + beta_schedule='linear', + n_timesteps=100, + ema_decay=0.995, + step_start_ema=1000, + update_ema_every=5, + lr=3e-4, + lr_decay=False, + lr_maxt=1000, + grad_norm=1.0, + ): + + self.model = MLP(state_dim=state_dim, action_dim=action_dim, device=device) + + self.actor = Diffusion(state_dim=state_dim, action_dim=action_dim, model=self.model, max_action=max_action, + beta_schedule=beta_schedule, n_timesteps=n_timesteps,).to(device) + self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=lr) + + self.lr_decay = lr_decay + self.grad_norm = grad_norm + + self.step = 0 + self.step_start_ema = step_start_ema + self.ema = EMA(ema_decay) + self.ema_model = copy.deepcopy(self.actor) + self.update_ema_every = update_ema_every + + self.critic = Critic(state_dim, action_dim).to(device) + self.critic_target = copy.deepcopy(self.critic) + self.critic_optimizer = torch.optim.Adam(self.critic.parameters(), lr=3e-4) + + if lr_decay: + self.actor_lr_scheduler = CosineAnnealingLR(self.actor_optimizer, T_max=lr_maxt, eta_min=0.) + self.critic_lr_scheduler = CosineAnnealingLR(self.critic_optimizer, T_max=lr_maxt, eta_min=0.) + + self.state_dim = state_dim + self.max_action = max_action + self.action_dim = action_dim + self.discount = discount + self.tau = tau + self.eta = eta # q_learning weight + self.device = device + self.max_q_backup = max_q_backup + + def step_ema(self): + if self.step < self.step_start_ema: + return + self.ema.update_model_average(self.ema_model, self.actor) + + def train(self, replay_buffer, iterations, batch_size=100, log_writer=None): + + metric = {'bc_loss': [], 'ql_loss': [], 'actor_loss': [], 'critic_loss': []} + for _ in range(iterations): + # Sample replay buffer / batch + state, action, next_state, reward, not_done = replay_buffer.sample(batch_size) + + """ Q Training """ + current_q1, current_q2 = self.critic(state, action) + + if self.max_q_backup: + next_state_rpt = torch.repeat_interleave(next_state, repeats=10, dim=0) + next_action_rpt = self.ema_model(next_state_rpt) + target_q1, target_q2 = self.critic_target(next_state_rpt, next_action_rpt) + target_q1 = target_q1.view(batch_size, 10).max(dim=1, keepdim=True)[0] + target_q2 = target_q2.view(batch_size, 10).max(dim=1, keepdim=True)[0] + target_q = torch.min(target_q1, target_q2) + else: + next_action = self.ema_model(next_state) + target_q1, target_q2 = self.critic_target(next_state, next_action) + target_q = torch.min(target_q1, target_q2) + + target_q = (reward + not_done * self.discount * target_q).detach() + + critic_loss = F.mse_loss(current_q1, target_q) + F.mse_loss(current_q2, target_q) + + self.critic_optimizer.zero_grad() + critic_loss.backward() + if self.grad_norm > 0: + critic_grad_norms = nn.utils.clip_grad_norm_(self.critic.parameters(), max_norm=self.grad_norm, norm_type=2) + self.critic_optimizer.step() + + """ Policy Training """ + bc_loss = self.actor.loss(action, state) + new_action = self.actor(state) + + q1_new_action, q2_new_action = self.critic(state, new_action) + if np.random.uniform() > 0.5: + q_loss = - q1_new_action.mean() / q2_new_action.abs().mean().detach() + else: + q_loss = - q2_new_action.mean() / q1_new_action.abs().mean().detach() + actor_loss = bc_loss + self.eta * q_loss + + self.actor_optimizer.zero_grad() + actor_loss.backward() + if self.grad_norm > 0: + actor_grad_norms = nn.utils.clip_grad_norm_(self.actor.parameters(), max_norm=self.grad_norm, norm_type=2) + self.actor_optimizer.step() + + + """ Step Target network """ + if self.step % self.update_ema_every == 0: + self.step_ema() + + for param, target_param in zip(self.critic.parameters(), self.critic_target.parameters()): + target_param.data.copy_(self.tau * param.data + (1 - self.tau) * target_param.data) + + self.step += 1 + + """ Log """ + if log_writer is not None: + if self.grad_norm > 0: + log_writer.add_scalar('Actor Grad Norm', actor_grad_norms.max().item(), self.step) + log_writer.add_scalar('Critic Grad Norm', critic_grad_norms.max().item(), self.step) + log_writer.add_scalar('BC Loss', bc_loss.item(), self.step) + log_writer.add_scalar('QL Loss', q_loss.item(), self.step) + log_writer.add_scalar('Critic Loss', critic_loss.item(), self.step) + log_writer.add_scalar('Target_Q Mean', target_q.mean().item(), self.step) + + metric['actor_loss'].append(actor_loss.item()) + metric['bc_loss'].append(bc_loss.item()) + metric['ql_loss'].append(q_loss.item()) + metric['critic_loss'].append(critic_loss.item()) + + if self.lr_decay: + self.actor_lr_scheduler.step() + self.critic_lr_scheduler.step() + + return metric + + def sample_action(self, state): + state = torch.FloatTensor(state.reshape(1, -1)).to(self.device) + state_rpt = torch.repeat_interleave(state, repeats=50, dim=0) + with torch.no_grad(): + action = self.actor.sample(state_rpt) + q_value = self.critic_target.q_min(state_rpt, action).flatten() + idx = torch.multinomial(F.softmax(q_value, dim=0), 1) + return action[idx].cpu().data.numpy().flatten() + + def save_model(self, dir, id=None): + if id is not None: + torch.save(self.actor.state_dict(), f'{dir}/actor_{id}.pth') + torch.save(self.critic.state_dict(), f'{dir}/critic_{id}.pth') + else: + torch.save(self.actor.state_dict(), f'{dir}/actor.pth') + torch.save(self.critic.state_dict(), f'{dir}/critic.pth') + + def load_model(self, dir, id=None): + if id is not None: + self.actor.load_state_dict(torch.load(f'{dir}/actor_{id}.pth')) + self.critic.load_state_dict(torch.load(f'{dir}/critic_{id}.pth')) + else: + self.actor.load_state_dict(torch.load(f'{dir}/actor.pth')) + self.critic.load_state_dict(torch.load(f'{dir}/critic.pth')) + + diff --git a/example/params_dql/actor_200.pth b/example/params_dql/actor_200.pth new file mode 100644 index 0000000..44f0362 Binary files /dev/null and b/example/params_dql/actor_200.pth differ diff --git a/example/params_dql/critic_200.pth b/example/params_dql/critic_200.pth new file mode 100644 index 0000000..5b8793b Binary files /dev/null and b/example/params_dql/critic_200.pth differ diff --git a/example/run_dql_in_carla.py b/example/run_dql_in_carla.py new file mode 100644 index 0000000..4ddb69c --- /dev/null +++ b/example/run_dql_in_carla.py @@ -0,0 +1,98 @@ +# -*- coding: utf-8 -*- +""" +Author: SilverWings +GitHub: https://github.com/silverwingsbot + +Simple example: Run a trained Diffusion_QL model in easycarla +""" + +import gym +import easycarla +import numpy as np +import torch +import os +from agents.ql_diffusion import Diffusion_QL + +# ===================== Helper Functions ===================== +def convert_obs_dict_to_vector(obs_dict): + """Convert observation dictionary to a flattened state vector.""" + return np.concatenate([ + obs_dict['ego_state'], # 9 dimensions + obs_dict['lane_info'], # 2 dimensions + obs_dict['lidar'], # 240 dimensions + obs_dict['nearby_vehicles'], # 20 dimensions + obs_dict['waypoints'] # 36 dimensions + ]).astype(np.float32) + +# ===================== Environment Configuration ===================== +carla_params = { + 'number_of_vehicles': 100, + 'number_of_walkers': 0, + 'dt': 0.1, # time interval between two frames + 'ego_vehicle_filter': 'vehicle.tesla.model3', # filter for defining ego vehicle + 'surrounding_vehicle_spawned_randomly': True, # Whether surrounding vehicles are spawned randomly (True) or set manually (False) + 'port': 2000, # connection port + 'town': 'Town03', # which town to simulate + 'max_time_episode': 1000, # maximum timesteps per episode + 'max_waypoints': 12, # maximum number of waypoints + 'visualize_waypoints': True, # Whether to visualize waypoints (default: True) + 'desired_speed': 8, # desired speed (m/s) + 'max_ego_spawn_times': 200, # maximum times to spawn ego vehicle + 'view_mode' : 'top', # 'top' for bird's-eye view, 'follow' for third-person view + 'traffic': 'off', # 'on' for normal traffic lights, 'off' for always green and frozen + 'lidar_max_range': 50.0, # Maximum LIDAR perception range (meters) + 'max_nearby_vehicles': 5, # Maximum number of nearby vehicles to observe +} + +# ===================== Initialize Environment ===================== +env = gym.make('carla-v0', params=carla_params) + +# ===================== Initialize Model ===================== +state_dim = 307 +action_dim = 3 +max_action = 1.0 +device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') + +model = Diffusion_QL( + state_dim=state_dim, + action_dim=action_dim, + max_action=max_action, + device=device, + discount=0.99, + tau=0.005, + eta=0.01, + beta_schedule='vp', + n_timesteps=5 +) + +# ===================== Load Pretrained Model ===================== +model_id = 200 # Model checkpoint ID to load +save_path = './params_dql' # Model checkpoint directory +model.load_model(save_path, id=model_id) +print(f"Successfully loaded model ID {model_id}") + +# ===================== Run One Episode ===================== +obs = env.reset() +done = False +step = 0 +episode_reward = 0.0 + +while not done: + obs_vec = convert_obs_dict_to_vector(obs) + action = model.sample_action(obs_vec) + + try: + next_obs, reward, cost, done, info = env.step(action) + except Exception as e: + print(f"[Error] Carla step failed: {e}") + obs = env.reset() + continue + + obs = next_obs + episode_reward += reward + step += 1 + + # Optional: add a delay for better visualization + # time.sleep(0.05) + +print(f"Episode finished. Total reward: {episode_reward:.2f}, Total steps: {step}") \ No newline at end of file diff --git a/example/utils/__init__.py b/example/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/example/utils/data_sampler.py b/example/utils/data_sampler.py new file mode 100644 index 0000000..5fefc43 --- /dev/null +++ b/example/utils/data_sampler.py @@ -0,0 +1,55 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import torch +import numpy as np + +class Data_Sampler(object): + def __init__(self, data, device, reward_tune='no'): + self.state = torch.from_numpy(data['observations']).float() + self.action = torch.from_numpy(data['actions']).float() + self.next_state = torch.from_numpy(data['next_observations']).float() + reward = torch.from_numpy(data['rewards']).reshape(-1, 1).float() + self.not_done = 1. - torch.from_numpy(data['dones']).reshape(-1, 1).float() + + self.size = self.state.shape[0] + self.state_dim = self.state.shape[1] + self.action_dim = self.action.shape[1] + self.device = device + + # ไฟ็•™ๅŽŸ reward ่ฐƒๆ•ด้€ป่พ‘ + if reward_tune == 'normalize': + reward = (reward - reward.mean()) / reward.std() + elif reward_tune == 'iql_antmaze': + reward = reward - 1.0 + elif reward_tune == 'iql_locomotion': + reward = iql_normalize(reward, self.not_done) + elif reward_tune == 'cql_antmaze': + reward = (reward - 0.5) * 4.0 + elif reward_tune == 'antmaze': + reward = (reward - 0.25) * 2.0 + + self.reward = reward + + def sample(self, batch_size): + ind = torch.randint(0, self.size, size=(batch_size,)) + return ( + self.state[ind].to(self.device), + self.action[ind].to(self.device), + self.next_state[ind].to(self.device), + self.reward[ind].to(self.device), + self.not_done[ind].to(self.device) + ) + +def iql_normalize(reward, not_done): + trajs_rt = [] + episode_return = 0.0 + for i in range(len(reward)): + episode_return += reward[i] + if not not_done[i]: + trajs_rt.append(episode_return) + episode_return = 0.0 + rt_max, rt_min = torch.max(torch.tensor(trajs_rt)), torch.min(torch.tensor(trajs_rt)) + reward /= (rt_max - rt_min) + reward *= 1000. + return reward diff --git a/example/utils/logger.py b/example/utils/logger.py new file mode 100644 index 0000000..ef11781 --- /dev/null +++ b/example/utils/logger.py @@ -0,0 +1,493 @@ +""" +Based on rllab's logger. + +https://github.com/rll/rllab +""" +from enum import Enum +from contextlib import contextmanager +import numpy as np +import os +import os.path as osp +import sys +import datetime +import dateutil.tz +import csv +import json +import pickle +import errno +from collections import OrderedDict +from numbers import Number +import os + +from tabulate import tabulate +import dateutil.tz +import os.path as osp + +def dict_to_safe_json(d): + """ + Convert each value in the dictionary into a JSON'able primitive. + :param d: + :return: + """ + new_d = {} + for key, item in d.items(): + if safe_json(item): + new_d[key] = item + else: + if isinstance(item, dict): + new_d[key] = dict_to_safe_json(item) + else: + new_d[key] = str(item) + return new_d + + +def safe_json(data): + if data is None: + return True + elif isinstance(data, (bool, int, float)): + return True + elif isinstance(data, (tuple, list)): + return all(safe_json(x) for x in data) + elif isinstance(data, dict): + return all(isinstance(k, str) and safe_json(v) for k, v in data.items()) + return False + +def create_exp_name(exp_prefix, exp_id=0, seed=0): + """ + Create a semi-unique experiment name that has a timestamp + :param exp_prefix: + :param exp_id: + :return: + """ + now = datetime.datetime.now(dateutil.tz.tzlocal()) + timestamp = now.strftime('%Y_%m_%d_%H_%M_%S') + return "%s_%s_%04d--s-%d" % (exp_prefix, timestamp, exp_id, seed) + +def create_log_dir( + exp_prefix, + exp_id=0, + seed=0, + base_log_dir=None, + include_exp_prefix_sub_dir=True, +): + """ + Creates and returns a unique log directory. + :param exp_prefix: All experiments with this prefix will have log + directories be under this directory. + :param exp_id: The number of the specific experiment run within this + experiment. + :param base_log_dir: The directory where all log should be saved. + :return: + """ + exp_name = create_exp_name(exp_prefix, exp_id=exp_id, + seed=seed) + if base_log_dir is None: + base_log_dir = './data' + if include_exp_prefix_sub_dir: + log_dir = osp.join(base_log_dir, exp_prefix.replace("_", "-"), exp_name) + else: + log_dir = osp.join(base_log_dir, exp_name) + if osp.exists(log_dir): + print("WARNING: Log directory already exists {}".format(log_dir), flush=True) + os.makedirs(log_dir, exist_ok=True) + return log_dir + + +def setup_logger( + exp_prefix="default", + variant=None, + text_log_file="debug.log", + variant_log_file="variant.json", + tabular_log_file="progress.csv", + snapshot_mode="last", + snapshot_gap=1, + log_tabular_only=False, + log_dir=None, + git_infos=None, + script_name=None, + **create_log_dir_kwargs +): + """ + Set up logger to have some reasonable default settings. + Will save log output to + based_log_dir/exp_prefix/exp_name. + exp_name will be auto-generated to be unique. + If log_dir is specified, then that directory is used as the output dir. + :param exp_prefix: The sub-directory for this specific experiment. + :param variant: + :param text_log_file: + :param variant_log_file: + :param tabular_log_file: + :param snapshot_mode: + :param log_tabular_only: + :param snapshot_gap: + :param log_dir: + :param git_infos: + :param script_name: If set, save the script name to this. + :return: + """ + first_time = log_dir is None + if first_time: + log_dir = create_log_dir(exp_prefix, **create_log_dir_kwargs) + + if variant is not None: + logger.log("Variant:") + logger.log(json.dumps(dict_to_safe_json(variant), indent=2)) + variant_log_path = osp.join(log_dir, variant_log_file) + logger.log_variant(variant_log_path, variant) + + tabular_log_path = osp.join(log_dir, tabular_log_file) + text_log_path = osp.join(log_dir, text_log_file) + + logger.add_text_output(text_log_path) + if first_time: + logger.add_tabular_output(tabular_log_path) + else: + logger._add_output(tabular_log_path, logger._tabular_outputs, + logger._tabular_fds, mode='a') + for tabular_fd in logger._tabular_fds: + logger._tabular_header_written.add(tabular_fd) + logger.set_snapshot_dir(log_dir) + logger.set_snapshot_mode(snapshot_mode) + logger.set_snapshot_gap(snapshot_gap) + logger.set_log_tabular_only(log_tabular_only) + exp_name = log_dir.split("/")[-1] + logger.push_prefix("[%s] " % exp_name) + + if script_name is not None: + with open(osp.join(log_dir, "script_name.txt"), "w") as f: + f.write(script_name) + return log_dir + + +def create_stats_ordered_dict( + name, + data, + stat_prefix=None, + always_show_all_stats=True, + exclude_max_min=False, +): + if stat_prefix is not None: + name = "{}{}".format(stat_prefix, name) + if isinstance(data, Number): + return OrderedDict({name: data}) + + if len(data) == 0: + return OrderedDict() + + if isinstance(data, tuple): + ordered_dict = OrderedDict() + for number, d in enumerate(data): + sub_dict = create_stats_ordered_dict( + "{0}_{1}".format(name, number), + d, + ) + ordered_dict.update(sub_dict) + return ordered_dict + + if isinstance(data, list): + try: + iter(data[0]) + except TypeError: + pass + else: + data = np.concatenate(data) + + if (isinstance(data, np.ndarray) and data.size == 1 + and not always_show_all_stats): + return OrderedDict({name: float(data)}) + + stats = OrderedDict([ + (name + ' Mean', np.mean(data)), + (name + ' Std', np.std(data)), + ]) + if not exclude_max_min: + stats[name + ' Max'] = np.max(data) + stats[name + ' Min'] = np.min(data) + return stats + + +class TerminalTablePrinter(object): + def __init__(self): + self.headers = None + self.tabulars = [] + + def print_tabular(self, new_tabular): + if self.headers is None: + self.headers = [x[0] for x in new_tabular] + else: + assert len(self.headers) == len(new_tabular) + self.tabulars.append([x[1] for x in new_tabular]) + self.refresh() + + def refresh(self): + import os + rows, columns = os.popen('stty size', 'r').read().split() + tabulars = self.tabulars[-(int(rows) - 3):] + sys.stdout.write("\x1b[2J\x1b[H") + sys.stdout.write(tabulate(tabulars, self.headers)) + sys.stdout.write("\n") + + +class MyEncoder(json.JSONEncoder): + def default(self, o): + if isinstance(o, type): + return {'$class': o.__module__ + "." + o.__name__} + elif isinstance(o, Enum): + return { + '$enum': o.__module__ + "." + o.__class__.__name__ + '.' + o.name + } + elif callable(o): + return { + '$function': o.__module__ + "." + o.__name__ + } + return json.JSONEncoder.default(self, o) + + +def mkdir_p(path): + try: + os.makedirs(path) + except OSError as exc: # Python >2.5 + if exc.errno == errno.EEXIST and os.path.isdir(path): + pass + else: + raise + + +class Logger(object): + def __init__(self): + self._prefixes = [] + self._prefix_str = '' + + self._tabular_prefixes = [] + self._tabular_prefix_str = '' + + self._tabular = [] + + self._text_outputs = [] + self._tabular_outputs = [] + + self._text_fds = {} + self._tabular_fds = {} + self._tabular_header_written = set() + + self._snapshot_dir = None + self._snapshot_mode = 'all' + self._snapshot_gap = 1 + + self._log_tabular_only = False + self._header_printed = False + self.table_printer = TerminalTablePrinter() + + def reset(self): + self.__init__() + + def _add_output(self, file_name, arr, fds, mode='a'): + if file_name not in arr: + mkdir_p(os.path.dirname(file_name)) + arr.append(file_name) + fds[file_name] = open(file_name, mode) + + def _remove_output(self, file_name, arr, fds): + if file_name in arr: + fds[file_name].close() + del fds[file_name] + arr.remove(file_name) + + def push_prefix(self, prefix): + self._prefixes.append(prefix) + self._prefix_str = ''.join(self._prefixes) + + def add_text_output(self, file_name): + self._add_output(file_name, self._text_outputs, self._text_fds, + mode='a') + + def remove_text_output(self, file_name): + self._remove_output(file_name, self._text_outputs, self._text_fds) + + def add_tabular_output(self, file_name, relative_to_snapshot_dir=False): + if relative_to_snapshot_dir: + file_name = osp.join(self._snapshot_dir, file_name) + self._add_output(file_name, self._tabular_outputs, self._tabular_fds, + mode='w') + + def remove_tabular_output(self, file_name, relative_to_snapshot_dir=False): + if relative_to_snapshot_dir: + file_name = osp.join(self._snapshot_dir, file_name) + if self._tabular_fds[file_name] in self._tabular_header_written: + self._tabular_header_written.remove(self._tabular_fds[file_name]) + self._remove_output(file_name, self._tabular_outputs, self._tabular_fds) + + def set_snapshot_dir(self, dir_name): + self._snapshot_dir = dir_name + + def get_snapshot_dir(self, ): + return self._snapshot_dir + + def get_snapshot_mode(self, ): + return self._snapshot_mode + + def set_snapshot_mode(self, mode): + self._snapshot_mode = mode + + def get_snapshot_gap(self, ): + return self._snapshot_gap + + def set_snapshot_gap(self, gap): + self._snapshot_gap = gap + + def set_log_tabular_only(self, log_tabular_only): + self._log_tabular_only = log_tabular_only + + def get_log_tabular_only(self, ): + return self._log_tabular_only + + def log(self, s, with_prefix=True, with_timestamp=True): + out = s + if with_prefix: + out = self._prefix_str + out + if with_timestamp: + now = datetime.datetime.now(dateutil.tz.tzlocal()) + timestamp = now.strftime('%y-%m-%d.%H:%M') # :%S + out = "%s|%s" % (timestamp, out) + if not self._log_tabular_only: + # Also log to stdout + print(out, flush=True) + for fd in list(self._text_fds.values()): + fd.write(out + '\n') + fd.flush() + sys.stdout.flush() + + def record_tabular(self, key, val): + self._tabular.append((self._tabular_prefix_str + str(key), str(val))) + + def record_dict(self, d, prefix=None): + if prefix is not None: + self.push_tabular_prefix(prefix) + for k, v in d.items(): + self.record_tabular(k, v) + if prefix is not None: + self.pop_tabular_prefix() + + def push_tabular_prefix(self, key): + self._tabular_prefixes.append(key) + self._tabular_prefix_str = ''.join(self._tabular_prefixes) + + def pop_tabular_prefix(self, ): + del self._tabular_prefixes[-1] + self._tabular_prefix_str = ''.join(self._tabular_prefixes) + + def save_extra_data(self, data, file_name='extra_data.pkl', mode='joblib'): + """ + Data saved here will always override the last entry + + :param data: Something pickle'able. + """ + file_name = osp.join(self._snapshot_dir, file_name) + if mode == 'joblib': + import joblib + joblib.dump(data, file_name, compress=3) + elif mode == 'pickle': + pickle.dump(data, open(file_name, "wb")) + else: + raise ValueError("Invalid mode: {}".format(mode)) + return file_name + + def get_table_dict(self, ): + return dict(self._tabular) + + def get_table_key_set(self, ): + return set(key for key, value in self._tabular) + + @contextmanager + def prefix(self, key): + self.push_prefix(key) + try: + yield + finally: + self.pop_prefix() + + @contextmanager + def tabular_prefix(self, key): + self.push_tabular_prefix(key) + yield + self.pop_tabular_prefix() + + def log_variant(self, log_file, variant_data): + mkdir_p(os.path.dirname(log_file)) + with open(log_file, "w") as f: + json.dump(variant_data, f, indent=2, sort_keys=True, cls=MyEncoder) + + def record_tabular_misc_stat(self, key, values, placement='back'): + if placement == 'front': + prefix = "" + suffix = key + else: + prefix = key + suffix = "" + if len(values) > 0: + self.record_tabular(prefix + "Average" + suffix, np.average(values)) + self.record_tabular(prefix + "Std" + suffix, np.std(values)) + self.record_tabular(prefix + "Median" + suffix, np.median(values)) + self.record_tabular(prefix + "Min" + suffix, np.min(values)) + self.record_tabular(prefix + "Max" + suffix, np.max(values)) + else: + self.record_tabular(prefix + "Average" + suffix, np.nan) + self.record_tabular(prefix + "Std" + suffix, np.nan) + self.record_tabular(prefix + "Median" + suffix, np.nan) + self.record_tabular(prefix + "Min" + suffix, np.nan) + self.record_tabular(prefix + "Max" + suffix, np.nan) + + def dump_tabular(self, *args, **kwargs): + wh = kwargs.pop("write_header", None) + if len(self._tabular) > 0: + if self._log_tabular_only: + self.table_printer.print_tabular(self._tabular) + else: + for line in tabulate(self._tabular).split('\n'): + self.log(line, *args, **kwargs) + tabular_dict = dict(self._tabular) + # Also write to the csv files + # This assumes that the keys in each iteration won't change! + for tabular_fd in list(self._tabular_fds.values()): + writer = csv.DictWriter(tabular_fd, + fieldnames=list(tabular_dict.keys())) + if wh or ( + wh is None and tabular_fd not in self._tabular_header_written): + writer.writeheader() + self._tabular_header_written.add(tabular_fd) + writer.writerow(tabular_dict) + tabular_fd.flush() + del self._tabular[:] + + def pop_prefix(self, ): + del self._prefixes[-1] + self._prefix_str = ''.join(self._prefixes) + + def save_itr_params(self, itr, params): + if self._snapshot_dir: + if self._snapshot_mode == 'all': + file_name = osp.join(self._snapshot_dir, 'itr_%d.pkl' % itr) + pickle.dump(params, open(file_name, "wb")) + elif self._snapshot_mode == 'last': + # override previous params + file_name = osp.join(self._snapshot_dir, 'params.pkl') + pickle.dump(params, open(file_name, "wb")) + elif self._snapshot_mode == "gap": + if itr % self._snapshot_gap == 0: + file_name = osp.join(self._snapshot_dir, 'itr_%d.pkl' % itr) + pickle.dump(params, open(file_name, "wb")) + elif self._snapshot_mode == "gap_and_last": + if itr % self._snapshot_gap == 0: + file_name = osp.join(self._snapshot_dir, 'itr_%d.pkl' % itr) + pickle.dump(params, open(file_name, "wb")) + file_name = osp.join(self._snapshot_dir, 'params.pkl') + pickle.dump(params, open(file_name, "wb")) + elif self._snapshot_mode == 'none': + pass + else: + raise NotImplementedError + + +logger = Logger() + diff --git a/example/utils/pytorch_util.py b/example/utils/pytorch_util.py new file mode 100644 index 0000000..19b4303 --- /dev/null +++ b/example/utils/pytorch_util.py @@ -0,0 +1,48 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import torch +import numpy as np + + +def soft_update_from_to(source, target, tau): + for target_param, param in zip(target.parameters(), source.parameters()): + target_param.data.copy_( + target_param.data * (1.0 - tau) + param.data * tau + ) + + +def copy_model_params_from_to(source, target): + for target_param, param in zip(target.parameters(), source.parameters()): + target_param.data.copy_(param.data) + + +def fanin_init(tensor, scale=1): + size = tensor.size() + if len(size) == 2: + fan_in = size[0] + elif len(size) > 2: + fan_in = np.prod(size[1:]) + else: + raise Exception("Shape must be have dimension at least 2.") + bound = scale / np.sqrt(fan_in) + return tensor.data.uniform_(-bound, bound) + + +def orthogonal_init(tensor, gain=0.01): + torch.nn.init.orthogonal_(tensor, gain=gain) + + +def fanin_init_weights_like(tensor): + size = tensor.size() + if len(size) == 2: + fan_in = size[0] + elif len(size) > 2: + fan_in = np.prod(size[1:]) + else: + raise Exception("Shape must be have dimension at least 2.") + bound = 1. / np.sqrt(fan_in) + new_tensor = torch.FloatTensor(tensor.size()) + new_tensor.uniform_(-bound, bound) + return new_tensor + diff --git a/example/utils/utils.py b/example/utils/utils.py new file mode 100644 index 0000000..be4380d --- /dev/null +++ b/example/utils/utils.py @@ -0,0 +1,184 @@ +# Copyright 2022 Twitter, Inc and Zhendong Wang. +# SPDX-License-Identifier: Apache-2.0 + +import time +import math +import torch +import numpy as np + + +def print_banner(s, separator="-", num_star=60): + print(separator * num_star, flush=True) + print(s, flush=True) + print(separator * num_star, flush=True) + + +class Progress: + + def __init__(self, total, name='Progress', ncol=3, max_length=20, indent=0, line_width=100, speed_update_freq=100): + self.total = total + self.name = name + self.ncol = ncol + self.max_length = max_length + self.indent = indent + self.line_width = line_width + self._speed_update_freq = speed_update_freq + + self._step = 0 + self._prev_line = '\033[F' + self._clear_line = ' ' * self.line_width + + self._pbar_size = self.ncol * self.max_length + self._complete_pbar = '#' * self._pbar_size + self._incomplete_pbar = ' ' * self._pbar_size + + self.lines = [''] + self.fraction = '{} / {}'.format(0, self.total) + + self.resume() + + def update(self, description, n=1): + self._step += n + if self._step % self._speed_update_freq == 0: + self._time0 = time.time() + self._step0 = self._step + self.set_description(description) + + def resume(self): + self._skip_lines = 1 + print('\n', end='') + self._time0 = time.time() + self._step0 = self._step + + def pause(self): + self._clear() + self._skip_lines = 1 + + def set_description(self, params=[]): + + if type(params) == dict: + params = sorted([ + (key, val) + for key, val in params.items() + ]) + + ############ + # Position # + ############ + self._clear() + + ########### + # Percent # + ########### + percent, fraction = self._format_percent(self._step, self.total) + self.fraction = fraction + + ######### + # Speed # + ######### + speed = self._format_speed(self._step) + + ########## + # Params # + ########## + num_params = len(params) + nrow = math.ceil(num_params / self.ncol) + params_split = self._chunk(params, self.ncol) + params_string, lines = self._format(params_split) + self.lines = lines + + description = '{} | {}{}'.format(percent, speed, params_string) + print(description) + self._skip_lines = nrow + 1 + + def append_description(self, descr): + self.lines.append(descr) + + def _clear(self): + position = self._prev_line * self._skip_lines + empty = '\n'.join([self._clear_line for _ in range(self._skip_lines)]) + print(position, end='') + print(empty) + print(position, end='') + + def _format_percent(self, n, total): + if total: + percent = n / float(total) + + complete_entries = int(percent * self._pbar_size) + incomplete_entries = self._pbar_size - complete_entries + + pbar = self._complete_pbar[:complete_entries] + self._incomplete_pbar[:incomplete_entries] + fraction = '{} / {}'.format(n, total) + string = '{} [{}] {:3d}%'.format(fraction, pbar, int(percent * 100)) + else: + fraction = '{}'.format(n) + string = '{} iterations'.format(n) + return string, fraction + + def _format_speed(self, n): + num_steps = n - self._step0 + t = time.time() - self._time0 + speed = num_steps / t + string = '{:.1f} Hz'.format(speed) + if num_steps > 0: + self._speed = string + return string + + def _chunk(self, l, n): + return [l[i:i + n] for i in range(0, len(l), n)] + + def _format(self, chunks): + lines = [self._format_chunk(chunk) for chunk in chunks] + lines.insert(0, '') + padding = '\n' + ' ' * self.indent + string = padding.join(lines) + return string, lines + + def _format_chunk(self, chunk): + line = ' | '.join([self._format_param(param) for param in chunk]) + return line + + def _format_param(self, param): + k, v = param + return '{} : {}'.format(k, v)[:self.max_length] + + def stamp(self): + if self.lines != ['']: + params = ' | '.join(self.lines) + string = '[ {} ] {}{} | {}'.format(self.name, self.fraction, params, self._speed) + self._clear() + print(string, end='\n') + self._skip_lines = 1 + else: + self._clear() + self._skip_lines = 0 + + def close(self): + self.pause() + + +class Silent: + + def __init__(self, *args, **kwargs): + pass + + def __getattr__(self, attr): + return lambda *args: None + + +class EarlyStopping(object): + def __init__(self, tolerance=5, min_delta=0): + self.tolerance = tolerance + self.min_delta = min_delta + self.counter = 0 + self.early_stop = False + + def __call__(self, train_loss, validation_loss): + if (validation_loss - train_loss) > self.min_delta: + self.counter += 1 + if self.counter >= self.tolerance: + return True + else: + self.counter = 0 + return False diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..4c4d00b --- /dev/null +++ b/requirements.txt @@ -0,0 +1,9 @@ +gym<=0.26.2 +numpy==1.24.4 +pandas==2.0.3 +torch==1.13.0+cu116 +torchaudio==0.13.0+cu116 +torchvision==0.14.0+cu116 +tqdm==4.64.1 +matplotlib==3.7.2 +carla>=0.9.13 diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..f2444cc --- /dev/null +++ b/setup.py @@ -0,0 +1,10 @@ +from setuptools import setup, find_packages + +setup( + name='easycarla', + version='0.1.0', + description='A simple and easy-to-use OpenAI Gym environment based on the CARLA simulator.', + author='SilverWings', + packages=find_packages(), + install_requires=['gym'] +) \ No newline at end of file