Compare commits
11 Commits
d0d8224957
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
| 657f699a42 | |||
|
32910a3a1b | ||
|
|
cebcbe6700 | ||
|
|
6a25d3371f | ||
|
|
cf6b91c963 | ||
|
|
c6775d7197 | ||
|
|
ed2a9d3388 | ||
|
|
69a3d7a4e4 | ||
|
|
6cda061ed8 | ||
|
|
06c3aee0e2 | ||
|
|
b95898e9f6 |
3
.github/workflows/main.yml
vendored
3
.github/workflows/main.yml
vendored
@@ -38,7 +38,6 @@ jobs:
|
|||||||
pip install tensorflow==2.11.0
|
pip install tensorflow==2.11.0
|
||||||
pip install protobuf==3.20
|
pip install protobuf==3.20
|
||||||
pip install cython
|
pip install cython
|
||||||
pip install numpy
|
|
||||||
pip install -e .
|
pip install -e .
|
||||||
pip install pytest
|
pip install pytest
|
||||||
pip install pytest-cov
|
pip install pytest-cov
|
||||||
@@ -48,7 +47,7 @@ jobs:
|
|||||||
cd metadrive
|
cd metadrive
|
||||||
pip install -e .
|
pip install -e .
|
||||||
cd ../
|
cd ../
|
||||||
|
pip install numpy==1.26.4
|
||||||
cd scenarionet/
|
cd scenarionet/
|
||||||
pytest --cov=./ --cov-config=.coveragerc --cov-report=xml -sv tests
|
pytest --cov=./ --cov-config=.coveragerc --cov-report=xml -sv tests
|
||||||
|
|
||||||
|
|||||||
@@ -55,7 +55,7 @@ pip install -e.
|
|||||||
|
|
||||||
# Install ScenarioNet
|
# Install ScenarioNet
|
||||||
cd ~/ # Go to the folder you want to host these two repos.
|
cd ~/ # Go to the folder you want to host these two repos.
|
||||||
git clone git@github.com:metadriverse/scenarionet.git
|
git clone https://github.com/metadriverse/scenarionet.git
|
||||||
cd scenarionet
|
cd scenarionet
|
||||||
pip install -e .
|
pip install -e .
|
||||||
```
|
```
|
||||||
|
|||||||
34
documentation/argoverse2.rst
Normal file
34
documentation/argoverse2.rst
Normal file
@@ -0,0 +1,34 @@
|
|||||||
|
#############################
|
||||||
|
Argoverse 2.0
|
||||||
|
#############################
|
||||||
|
|
||||||
|
| Website: https://www.argoverse.org/index.html
|
||||||
|
| Download: https://www.argoverse.org/av2.html#download-link
|
||||||
|
|
||||||
|
|
||||||
|
Argoverse 2 is a collection of open-source autonomous driving data and high-definition (HD) maps from six U.S. cities: Austin, Detroit, Miami, Pittsburgh, Palo Alto, and Washington, D.C. This release builds upon the initial launch of Argoverse (“Argoverse 1”), which was among the first data releases of its kind to include HD maps for machine learning and computer vision research.
|
||||||
|
|
||||||
|
Argoverse 2 Motion Forecasting Dataset: contains 250,000 scenarios with trajectory data for many object types. This dataset improves upon the Argoverse 1 Motion Forecasting Dataset.
|
||||||
|
|
||||||
|
|
||||||
|
1. Install av2
|
||||||
|
==========================
|
||||||
|
|
||||||
|
First of all, we have to install the ``av2`` package.
|
||||||
|
|
||||||
|
You can following the instructions here: https://argoverse.github.io/user-guide/getting_started.html#downloading-the-data
|
||||||
|
|
||||||
|
2. Download Data
|
||||||
|
===========================
|
||||||
|
|
||||||
|
You can following the instructions here: https://argoverse.github.io/user-guide/getting_started.html#downloading-the-data
|
||||||
|
|
||||||
|
3. Build av2 Database
|
||||||
|
============================
|
||||||
|
|
||||||
|
python -m scenarionet.convert_argoverse2 -d /path/to/your/database --raw_data_path /path/to/your/raw_data
|
||||||
|
|
||||||
|
Known Issues: Argoverse2
|
||||||
|
======================
|
||||||
|
|
||||||
|
N/A
|
||||||
@@ -21,7 +21,8 @@ We will fix it as best as we can and record it in the troubleshooting section fo
|
|||||||
- :ref:`PG`
|
- :ref:`PG`
|
||||||
- :ref:`lyft`
|
- :ref:`lyft`
|
||||||
- :ref:`new_data`
|
- :ref:`new_data`
|
||||||
|
- :ref:`argoverse2`
|
||||||
|
- :ref:`vod`
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -38,7 +38,7 @@ For Waymo data, we already have the parser in ScenarioNet so just install the Te
|
|||||||
conda install protobuf==3.20
|
conda install protobuf==3.20
|
||||||
|
|
||||||
.. note::
|
.. note::
|
||||||
You may fail to install ``protobuf`` if using ``pip install protobuf==3.20``.
|
You may fail to install ``protobuf`` if using ``pip install protobuf==3.20``. If so, install via ``conda install protobuf=3.20``.
|
||||||
|
|
||||||
For other datasets like nuPlan and nuScenes, you need to setup `nuplan-devkit <https://github.com/motional/nuplan-devkit>`_ and `nuscenes-devkit <https://github.com/nutonomy/nuscenes-devkit>`_ respectively.
|
For other datasets like nuPlan and nuScenes, you need to setup `nuplan-devkit <https://github.com/motional/nuplan-devkit>`_ and `nuscenes-devkit <https://github.com/nutonomy/nuscenes-devkit>`_ respectively.
|
||||||
Guidance on how to setup these datasets and connect them with ScenarioNet can be found at :ref:`datasets`.
|
Guidance on how to setup these datasets and connect them with ScenarioNet can be found at :ref:`datasets`.
|
||||||
|
|||||||
@@ -54,6 +54,8 @@ Please feel free to contact us if you have any suggestion or idea!
|
|||||||
waymo.rst
|
waymo.rst
|
||||||
PG.rst
|
PG.rst
|
||||||
lyft.rst
|
lyft.rst
|
||||||
|
argoverse2.rst
|
||||||
|
vod.rst
|
||||||
new_data.rst
|
new_data.rst
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -162,6 +162,55 @@ However, Lyft is now a part of Woven Planet and the new data has to be parsed vi
|
|||||||
We are working on support this new toolkit to support the new Lyft dataset.
|
We are working on support this new toolkit to support the new Lyft dataset.
|
||||||
Detailed guide is available at Section :ref:`nuscenes`.
|
Detailed guide is available at Section :ref:`nuscenes`.
|
||||||
|
|
||||||
|
Convert VoD
|
||||||
|
------------------------------------
|
||||||
|
|
||||||
|
.. code-block:: text
|
||||||
|
|
||||||
|
python -m scenarionet.convert_vod [-h] [--database_path DATABASE_PATH]
|
||||||
|
[--dataset_name DATASET_NAME]
|
||||||
|
[--split
|
||||||
|
{v1.0-trainval,v1.0-test,train,train_val,val,test}]
|
||||||
|
[--dataroot DATAROOT] [--map_radius MAP_RADIUS]
|
||||||
|
[--future FUTURE] [--past PAST] [--overwrite]
|
||||||
|
[--num_workers NUM_WORKERS]
|
||||||
|
|
||||||
|
Build database from VOD scenarios
|
||||||
|
|
||||||
|
optional arguments:
|
||||||
|
-h, --help show this help message and exit
|
||||||
|
--database_path DATABASE_PATH, -d DATABASE_PATH
|
||||||
|
directory, The path to place the data
|
||||||
|
--dataset_name DATASET_NAME, -n DATASET_NAME
|
||||||
|
Dataset name, will be used to generate scenario files
|
||||||
|
--split
|
||||||
|
{v1.0-trainval,v1.0-test,train,train_val,val,test}
|
||||||
|
Which splits of VOD data should be used. If set to
|
||||||
|
['v1.0-trainval', 'v1.0-test'], it will
|
||||||
|
convert the full log into scenarios with 20 second episode
|
||||||
|
length. If set to ['train', 'train_val', 'val', 'test'],
|
||||||
|
it will convert segments used for VOD prediction challenge
|
||||||
|
to scenarios, resulting in more converted scenarios.
|
||||||
|
Generally, you should choose this parameter from
|
||||||
|
['v1.0-trainval', 'v1.0-test'] to get complete
|
||||||
|
scenarios for planning unless you want to use the
|
||||||
|
converted scenario files for prediction task.
|
||||||
|
--dataroot DATAROOT The path of vod data
|
||||||
|
--map_radius MAP_RADIUS The size of map
|
||||||
|
--future FUTURE 3 seconds by default. How many future seconds to
|
||||||
|
predict. Only available if split is chosen from
|
||||||
|
['train', 'train_val', 'val', 'test']
|
||||||
|
--past PAST 0.5 seconds by default. How many past seconds are
|
||||||
|
used for prediction. Only available if split is
|
||||||
|
chosen from ['train', 'train_val', 'val', 'test']
|
||||||
|
--overwrite If the database_path exists, whether to overwrite it
|
||||||
|
--num_workers NUM_WORKERS number of workers to use
|
||||||
|
|
||||||
|
|
||||||
|
This script converts the View-of-Delft Prediction (VoD) dataset into our scenario descriptions.
|
||||||
|
You will need to install ``vod-devkit`` and download the source data from https://intelligent-vehicles.org/datasets/view-of-delft/.
|
||||||
|
Detailed guide is available at Section :ref:`vod`.
|
||||||
|
|
||||||
Convert PG
|
Convert PG
|
||||||
-------------------------
|
-------------------------
|
||||||
|
|
||||||
|
|||||||
109
documentation/vod.rst
Normal file
109
documentation/vod.rst
Normal file
@@ -0,0 +1,109 @@
|
|||||||
|
#############################
|
||||||
|
View-of-Delft (VoD)
|
||||||
|
#############################
|
||||||
|
|
||||||
|
| Website: https://intelligent-vehicles.org/datasets/view-of-delft/
|
||||||
|
| Download: https://intelligent-vehicles.org/datasets/view-of-delft/ (Registration required)
|
||||||
|
| Papers:
|
||||||
|
Detection dataset: https://ieeexplore.ieee.org/document/9699098
|
||||||
|
Prediction dataset: https://ieeexplore.ieee.org/document/10493110
|
||||||
|
|
||||||
|
The View-of-Delft (VoD) dataset is a novel automotive dataset recorded in Delft,
|
||||||
|
the Netherlands. It contains 8600+ frames of synchronized and calibrated
|
||||||
|
64-layer LiDAR-, (stereo) camera-, and 3+1D (range, azimuth, elevation, +
|
||||||
|
Doppler) radar-data acquired in complex, urban traffic. It consists of 123100+
|
||||||
|
3D bounding box annotations of both moving and static objects, including 26500+
|
||||||
|
pedestrian, 10800 cyclist and 26900+ car labels. It additionally contains
|
||||||
|
semantic map annotations and accurate ego-vehicle localization data.
|
||||||
|
|
||||||
|
Benchmarks for detection and prediction tasks are released for the dataset. See
|
||||||
|
the sections below for details on these benchmarks.
|
||||||
|
|
||||||
|
**Detection**:
|
||||||
|
An object detection benchmark is available for researchers to develop and
|
||||||
|
evaluate their models on the VoD dataset. At the time of publication, this
|
||||||
|
benchmark was the largest automotive multi-class object detection dataset
|
||||||
|
containing 3+1D radar data, and the only dataset containing high-end (64-layer)
|
||||||
|
LiDAR and (any kind of) radar data at the same time.
|
||||||
|
|
||||||
|
**Prediction**:
|
||||||
|
A trajectory prediction benchmark is publicly available to enable research
|
||||||
|
on urban multi-class trajectory prediction. This benchmark contains challenging
|
||||||
|
prediction cases in the historic city center of Delft with a high proportion of
|
||||||
|
Vulnerable Road Users (VRUs), such as pedestrians and cyclists. Semantic map
|
||||||
|
annotations for road elements such as lanes, sidewalks, and crosswalks are
|
||||||
|
provided as context for prediction models.
|
||||||
|
|
||||||
|
1. Install VoD Prediction Toolkit
|
||||||
|
=================================
|
||||||
|
|
||||||
|
We will use the VoD Prediction toolkit to convert the data.
|
||||||
|
First of all, we have to install the ``vod-devkit``.
|
||||||
|
|
||||||
|
.. code-block:: bash
|
||||||
|
|
||||||
|
# install from github (Recommend)
|
||||||
|
git clone git@github.com:tudelft-iv/view-of-delft-prediction-devkit.git
|
||||||
|
cd vod-devkit
|
||||||
|
pip install -e .
|
||||||
|
|
||||||
|
# or install from PyPI
|
||||||
|
pip install vod-devkit
|
||||||
|
|
||||||
|
By installing from github, you can access examples and source code the toolkit.
|
||||||
|
The examples are useful to verify whether the installation and dataset setup is correct or not.
|
||||||
|
|
||||||
|
|
||||||
|
2. Download VoD Data
|
||||||
|
==============================
|
||||||
|
|
||||||
|
The official instruction is available at https://intelligent-vehicles.org/datasets/view-of-delft/.
|
||||||
|
Here we provide a simplified installation procedure.
|
||||||
|
|
||||||
|
First of all, please fill in the access form on vod website: https://intelligent-vehicles.org/datasets/view-of-delft/.
|
||||||
|
The maintainers will send the data link to your email. Download and unzip the file named ``view_of_delft_prediction_PUBLIC.zip``.
|
||||||
|
|
||||||
|
Secondly, all files should be organized to the following structure::
|
||||||
|
|
||||||
|
/vod/data/path/
|
||||||
|
├── maps/
|
||||||
|
| └──expansion/
|
||||||
|
├── v1.0-trainval/
|
||||||
|
| ├──attribute.json
|
||||||
|
| ├──calibrated_sensor.json
|
||||||
|
| ├──map.json
|
||||||
|
| ├──log.json
|
||||||
|
| ├──ego_pose.json
|
||||||
|
| └──...
|
||||||
|
└── v1.0-test/
|
||||||
|
|
||||||
|
**Note**: The sensor data is currently not available in the Prediction dataset, but will be released in the near future.
|
||||||
|
|
||||||
|
The ``/vod/data/path`` should be ``/data/sets/vod`` by default according to the official instructions,
|
||||||
|
allowing the ``vod-devkit`` to find it.
|
||||||
|
But you can still place it to any other places and:
|
||||||
|
|
||||||
|
- build a soft link connect your data folder and ``/data/sets/vod``
|
||||||
|
- or specify the ``dataroot`` when calling vod APIs and our convertors.
|
||||||
|
|
||||||
|
|
||||||
|
After this step, the examples in ``vod-devkit`` is supposed to work well.
|
||||||
|
Please try ``view-of-delft-prediction-devkit/tutorials/vod_tutorial.ipynb`` and see if the demo can successfully run.
|
||||||
|
|
||||||
|
3. Build VoD Database
|
||||||
|
===========================
|
||||||
|
|
||||||
|
After setup the raw data, convertors in ScenarioNet can read the raw data, convert scenario format and build the database.
|
||||||
|
Here we take converting raw data in ``v1.0-trainval`` as an example::
|
||||||
|
|
||||||
|
python -m scenarionet.convert_vod -d /path/to/your/database --split v1.0-trainval --dataroot /vod/data/path
|
||||||
|
|
||||||
|
The ``split`` is to determine which split to convert. ``dataroot`` is set to ``/data/sets/vod`` by default,
|
||||||
|
but you need to specify it if your data is stored in any other directory.
|
||||||
|
Now all converted scenarios will be placed at ``/path/to/your/database`` and are ready to be used in your work.
|
||||||
|
|
||||||
|
|
||||||
|
Known Issues: VoD
|
||||||
|
=======================
|
||||||
|
|
||||||
|
N/A
|
||||||
@@ -35,7 +35,7 @@ First of all, we have to install tensorflow and Protobuf::
|
|||||||
conda install protobuf==3.20
|
conda install protobuf==3.20
|
||||||
|
|
||||||
.. note::
|
.. note::
|
||||||
You may fail to install ``protobuf`` if using ``pip install protobuf==3.20``.
|
You may fail to install ``protobuf`` if using ``pip install protobuf==3.20``. If so, install via ``conda install protobuf=3.20``.
|
||||||
|
|
||||||
|
|
||||||
2. Download TFRecord
|
2. Download TFRecord
|
||||||
@@ -45,7 +45,12 @@ Waymo motion dataset is at `Google Cloud <https://console.cloud.google.com/stora
|
|||||||
For downloading all datasets, ``gsutil`` is required.
|
For downloading all datasets, ``gsutil`` is required.
|
||||||
The installation tutorial is at https://cloud.google.com/storage/docs/gsutil_install.
|
The installation tutorial is at https://cloud.google.com/storage/docs/gsutil_install.
|
||||||
|
|
||||||
After this, you can access all data and download them to current directory ``./`` by::
|
Login you google account via::
|
||||||
|
|
||||||
|
gcloud init
|
||||||
|
|
||||||
|
|
||||||
|
After this, you can access all data and download them to current directory ``./`` by (don't forget the dot!)::
|
||||||
|
|
||||||
gsutil -m cp -r "gs://waymo_open_dataset_motion_v_1_2_0/uncompressed/scenario" .
|
gsutil -m cp -r "gs://waymo_open_dataset_motion_v_1_2_0/uncompressed/scenario" .
|
||||||
|
|
||||||
@@ -74,12 +79,18 @@ The downloaded data should be stored in a directory like this::
|
|||||||
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||||||
|
|
||||||
Run the following command to extract scenarios in any directory containing ``tfrecord``.
|
Run the following command to extract scenarios in any directory containing ``tfrecord``.
|
||||||
|
|
||||||
|
|
||||||
Here we take converting raw data in ``training_20s`` as an example::
|
Here we take converting raw data in ``training_20s`` as an example::
|
||||||
|
|
||||||
python -m scenarionet.convert_waymo -d /path/to/your/database --raw_data_path ./waymo/training_20s --num_files=1000
|
python -m scenarionet.convert_waymo -d /path/to/your/database --raw_data_path ./waymo/training_20s --num_workers 64
|
||||||
|
|
||||||
Now all converted scenarios will be placed at ``/path/to/your/database`` and are ready to be used in your work.
|
Now all converted scenarios will be placed at ``/path/to/your/database`` and are ready to be used in your work.
|
||||||
|
|
||||||
|
.. note::
|
||||||
|
When running the conversion, please double check whether GPU is being used. This converter should NOT use GPU.
|
||||||
|
We have disable GPU usage by ``os.environ["CUDA_VISIBLE_DEVICES"] = ""``.
|
||||||
|
|
||||||
Known Issues: Waymo
|
Known Issues: Waymo
|
||||||
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||||||
N/A
|
N/A
|
||||||
|
|||||||
@@ -53,6 +53,18 @@ class ScenarioFilter:
|
|||||||
def no_traffic_light(metadata, file_path):
|
def no_traffic_light(metadata, file_path):
|
||||||
return metadata[SD.SUMMARY.NUMBER_SUMMARY][SD.SUMMARY.NUM_TRAFFIC_LIGHTS] == 0
|
return metadata[SD.SUMMARY.NUMBER_SUMMARY][SD.SUMMARY.NUM_TRAFFIC_LIGHTS] == 0
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def no_pedestrian(metadata, file_path):
|
||||||
|
"""Return True if the scenario has no pedestrians"""
|
||||||
|
num = metadata[SD.SUMMARY.NUMBER_SUMMARY][SD.SUMMARY.NUM_OBJECTS_EACH_TYPE].get("PEDESTRIAN", 0)
|
||||||
|
return num == 0
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def no_cyclist(metadata, file_path):
|
||||||
|
"""Return True if the scenario has no cyclists"""
|
||||||
|
num = metadata[SD.SUMMARY.NUMBER_SUMMARY][SD.SUMMARY.NUM_OBJECTS_EACH_TYPE].get("CYCLIST", 0)
|
||||||
|
return num == 0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def no_overpass(metadata, file_path):
|
def no_overpass(metadata, file_path):
|
||||||
"""
|
"""
|
||||||
|
|||||||
@@ -11,7 +11,7 @@ from typing import Callable, List
|
|||||||
import tqdm
|
import tqdm
|
||||||
from metadrive.scenario.scenario_description import ScenarioDescription
|
from metadrive.scenario.scenario_description import ScenarioDescription
|
||||||
|
|
||||||
from scenarionet.common_utils import save_summary_anda_mapping
|
from scenarionet.common_utils import save_summary_and_mapping
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
@@ -110,7 +110,7 @@ def merge_database(
|
|||||||
summaries.pop(file)
|
summaries.pop(file)
|
||||||
mappings.pop(file)
|
mappings.pop(file)
|
||||||
if save:
|
if save:
|
||||||
save_summary_anda_mapping(summary_file, mapping_file, summaries, mappings)
|
save_summary_and_mapping(summary_file, mapping_file, summaries, mappings)
|
||||||
|
|
||||||
return summaries, mappings
|
return summaries, mappings
|
||||||
|
|
||||||
@@ -144,7 +144,7 @@ def copy_database(from_path, to_path, exist_ok=False, overwrite=False, copy_raw_
|
|||||||
rel_path = mappings[scenario_file]
|
rel_path = mappings[scenario_file]
|
||||||
shutil.copyfile(os.path.join(to_path, rel_path, scenario_file), os.path.join(to_path, scenario_file))
|
shutil.copyfile(os.path.join(to_path, rel_path, scenario_file), os.path.join(to_path, scenario_file))
|
||||||
mappings = {key: "./" for key in summaries.keys()}
|
mappings = {key: "./" for key in summaries.keys()}
|
||||||
save_summary_anda_mapping(summary_file, mapping_file, summaries, mappings)
|
save_summary_and_mapping(summary_file, mapping_file, summaries, mappings)
|
||||||
|
|
||||||
if remove_source:
|
if remove_source:
|
||||||
if ScenarioDescription.DATASET.MAPPING_FILE in files and ScenarioDescription.DATASET.SUMMARY_FILE in files \
|
if ScenarioDescription.DATASET.MAPPING_FILE in files and ScenarioDescription.DATASET.SUMMARY_FILE in files \
|
||||||
@@ -204,6 +204,6 @@ def split_database(
|
|||||||
selected_summary[scenario] = summaries[scenario]
|
selected_summary[scenario] = summaries[scenario]
|
||||||
selected_mapping[scenario] = os.path.relpath(osp.join(abs_dir_path, mappings[scenario]), output_abs_path)
|
selected_mapping[scenario] = os.path.relpath(osp.join(abs_dir_path, mappings[scenario]), output_abs_path)
|
||||||
|
|
||||||
save_summary_anda_mapping(summary_file, mapping_file, selected_summary, selected_mapping)
|
save_summary_and_mapping(summary_file, mapping_file, selected_summary, selected_mapping)
|
||||||
|
|
||||||
return summaries, mappings
|
return summaries, mappings
|
||||||
|
|||||||
@@ -1,9 +1,12 @@
|
|||||||
|
import logging
|
||||||
import os.path
|
import os.path
|
||||||
import pickle
|
import pickle
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from metadrive.scenario import utils as sd_utils
|
from metadrive.scenario import utils as sd_utils
|
||||||
|
|
||||||
|
logger = logging.getLogger(__file__)
|
||||||
|
|
||||||
|
|
||||||
def recursive_equal(data1, data2, need_assert=False):
|
def recursive_equal(data1, data2, need_assert=False):
|
||||||
from metadrive.utils.config import Config
|
from metadrive.utils.config import Config
|
||||||
@@ -66,12 +69,12 @@ def dict_recursive_remove_array_and_set(d):
|
|||||||
return d
|
return d
|
||||||
|
|
||||||
|
|
||||||
def save_summary_anda_mapping(summary_file_path, mapping_file_path, summary, mapping):
|
def save_summary_and_mapping(summary_file_path, mapping_file_path, summary, mapping):
|
||||||
with open(summary_file_path, "wb") as file:
|
with open(summary_file_path, "wb") as file:
|
||||||
pickle.dump(dict_recursive_remove_array_and_set(summary), file)
|
pickle.dump(dict_recursive_remove_array_and_set(summary), file)
|
||||||
with open(mapping_file_path, "wb") as file:
|
with open(mapping_file_path, "wb") as file:
|
||||||
pickle.dump(mapping, file)
|
pickle.dump(mapping, file)
|
||||||
print(
|
logging.info(
|
||||||
"\n ================ Dataset Summary and Mapping are saved at: {} "
|
"\n ================ Dataset Summary and Mapping are saved at: {} "
|
||||||
"================ \n".format(summary_file_path)
|
"================ \n".format(summary_file_path)
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1,16 +1,19 @@
|
|||||||
desc = "Build database from synthetic or procedurally generated scenarios"
|
desc = "Build database from synthetic or procedurally generated scenarios"
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
import pkg_resources # for suppress warning
|
|
||||||
import argparse
|
import argparse
|
||||||
import os.path
|
import os.path
|
||||||
|
import os
|
||||||
|
|
||||||
import metadrive
|
import metadrive
|
||||||
|
import tensorflow as tf
|
||||||
|
|
||||||
from scenarionet import SCENARIONET_DATASET_PATH
|
from scenarionet import SCENARIONET_DATASET_PATH
|
||||||
from scenarionet.converter.pg.utils import get_pg_scenarios, convert_pg_scenario
|
from scenarionet.converter.pg.utils import get_pg_scenarios, convert_pg_scenario
|
||||||
from scenarionet.converter.utils import write_to_directory
|
from scenarionet.converter.utils import write_to_directory
|
||||||
|
|
||||||
|
tf.config.experimental.set_visible_devices([], "GPU")
|
||||||
|
|
||||||
# For the PG environment config, see: scenarionet/converter/pg/utils.py:6
|
# For the PG environment config, see: scenarionet/converter/pg/utils.py:6
|
||||||
parser = argparse.ArgumentParser(description=desc)
|
parser = argparse.ArgumentParser(description=desc)
|
||||||
parser.add_argument(
|
parser.add_argument(
|
||||||
|
|||||||
95
scenarionet/convert_vod.py
Normal file
95
scenarionet/convert_vod.py
Normal file
@@ -0,0 +1,95 @@
|
|||||||
|
desc = "Build database from VOD scenarios"
|
||||||
|
|
||||||
|
prediction_split = ["train", "train_val", "val", "test"]
|
||||||
|
scene_split = ["v1.0-trainval", "v1.0-test"]
|
||||||
|
|
||||||
|
split_to_scene = {
|
||||||
|
"train": "v1.0-trainval",
|
||||||
|
"train_val": "v1.0-trainval",
|
||||||
|
"val": "v1.0-trainval",
|
||||||
|
"test": "v1.0-test",
|
||||||
|
}
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
import pkg_resources # for suppress warning
|
||||||
|
import argparse
|
||||||
|
import os.path
|
||||||
|
from functools import partial
|
||||||
|
from scenarionet import SCENARIONET_DATASET_PATH
|
||||||
|
from scenarionet.converter.vod.utils import (
|
||||||
|
convert_vod_scenario,
|
||||||
|
get_vod_scenarios,
|
||||||
|
get_vod_prediction_split,
|
||||||
|
)
|
||||||
|
from scenarionet.converter.utils import write_to_directory
|
||||||
|
|
||||||
|
parser = argparse.ArgumentParser(description=desc)
|
||||||
|
parser.add_argument(
|
||||||
|
"--database_path",
|
||||||
|
"-d",
|
||||||
|
default=os.path.join(SCENARIONET_DATASET_PATH, "vod"),
|
||||||
|
help="directory, The path to place the data",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--dataset_name",
|
||||||
|
"-n",
|
||||||
|
default="vod",
|
||||||
|
help="Dataset name, will be used to generate scenario files",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--split",
|
||||||
|
default="v1.0-trainval",
|
||||||
|
choices=scene_split + prediction_split,
|
||||||
|
help="Which splits of VOD data should be used. If set to {}, it will convert the full log into scenarios"
|
||||||
|
" with 20 second episode length. If set to {}, it will convert segments used for VOD prediction"
|
||||||
|
" challenge to scenarios, resulting in more converted scenarios. Generally, you should choose this "
|
||||||
|
" parameter from {} to get complete scenarios for planning unless you want to use the converted scenario "
|
||||||
|
" files for prediction task.".format(scene_split, prediction_split, scene_split),
|
||||||
|
)
|
||||||
|
parser.add_argument("--dataroot", default="/data/sets/vod", help="The path of vod data")
|
||||||
|
parser.add_argument("--map_radius", default=500, type=float, help="The size of map")
|
||||||
|
parser.add_argument(
|
||||||
|
"--future",
|
||||||
|
default=3,
|
||||||
|
type=float,
|
||||||
|
help="3 seconds by default. How many future seconds to predict. Only "
|
||||||
|
"available if split is chosen from {}".format(prediction_split),
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--past",
|
||||||
|
default=0.5,
|
||||||
|
type=float,
|
||||||
|
help="0.5 seconds by default. How many past seconds are used for prediction."
|
||||||
|
" Only available if split is chosen from {}".format(prediction_split),
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--overwrite",
|
||||||
|
action="store_true",
|
||||||
|
help="If the database_path exists, whether to overwrite it",
|
||||||
|
)
|
||||||
|
parser.add_argument("--num_workers", type=int, default=8, help="number of workers to use")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
overwrite = args.overwrite
|
||||||
|
dataset_name = args.dataset_name
|
||||||
|
output_path = args.database_path
|
||||||
|
version = args.split
|
||||||
|
|
||||||
|
if version in scene_split:
|
||||||
|
scenarios, vods = get_vod_scenarios(args.dataroot, version, args.num_workers)
|
||||||
|
else:
|
||||||
|
scenarios, vods = get_vod_prediction_split(args.dataroot, version, args.past, args.future, args.num_workers)
|
||||||
|
write_to_directory(
|
||||||
|
convert_func=convert_vod_scenario,
|
||||||
|
scenarios=scenarios,
|
||||||
|
output_path=output_path,
|
||||||
|
dataset_version=version,
|
||||||
|
dataset_name=dataset_name,
|
||||||
|
overwrite=overwrite,
|
||||||
|
num_workers=args.num_workers,
|
||||||
|
vodelft=vods,
|
||||||
|
past=[args.past for _ in range(args.num_workers)],
|
||||||
|
future=[args.future for _ in range(args.num_workers)],
|
||||||
|
prediction=[version in prediction_split for _ in range(args.num_workers)],
|
||||||
|
map_radius=[args.map_radius for _ in range(args.num_workers)],
|
||||||
|
)
|
||||||
@@ -1,15 +1,18 @@
|
|||||||
desc = "Build database from Waymo scenarios"
|
desc = "Build database from Waymo scenarios"
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
import pkg_resources # for suppress warning
|
|
||||||
import shutil
|
import shutil
|
||||||
import argparse
|
import argparse
|
||||||
import logging
|
import logging
|
||||||
import os
|
import os
|
||||||
|
import tensorflow as tf
|
||||||
|
|
||||||
from scenarionet import SCENARIONET_DATASET_PATH, SCENARIONET_REPO_PATH
|
from scenarionet import SCENARIONET_DATASET_PATH, SCENARIONET_REPO_PATH
|
||||||
from scenarionet.converter.utils import write_to_directory
|
from scenarionet.converter.utils import write_to_directory
|
||||||
from scenarionet.converter.waymo.utils import convert_waymo_scenario, get_waymo_scenarios, preprocess_waymo_scenarios
|
from scenarionet.converter.waymo.utils import convert_waymo_scenario, get_waymo_scenarios, \
|
||||||
|
preprocess_waymo_scenarios
|
||||||
|
|
||||||
|
tf.config.experimental.set_visible_devices([], "GPU")
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
@@ -36,14 +39,14 @@ if __name__ == '__main__':
|
|||||||
default=0,
|
default=0,
|
||||||
type=int,
|
type=int,
|
||||||
help="Control how many files to use. We will list all files in the raw data folder "
|
help="Control how many files to use. We will list all files in the raw data folder "
|
||||||
"and select files[start_file_index: start_file_index+num_files]"
|
"and select files[start_file_index: start_file_index+num_files]. Default: 0."
|
||||||
)
|
)
|
||||||
parser.add_argument(
|
parser.add_argument(
|
||||||
"--num_files",
|
"--num_files",
|
||||||
default=1000,
|
default=None,
|
||||||
type=int,
|
type=int,
|
||||||
help="Control how many files to use. We will list all files in the raw data folder "
|
help="Control how many files to use. We will list all files in the raw data folder "
|
||||||
"and select files[start_file_index: start_file_index+num_files]"
|
"and select files[start_file_index: start_file_index+num_files]. Default: None, will read all files."
|
||||||
)
|
)
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
|
|
||||||
@@ -65,6 +68,12 @@ if __name__ == '__main__':
|
|||||||
waymo_data_directory = os.path.join(SCENARIONET_DATASET_PATH, args.raw_data_path)
|
waymo_data_directory = os.path.join(SCENARIONET_DATASET_PATH, args.raw_data_path)
|
||||||
files = get_waymo_scenarios(waymo_data_directory, args.start_file_index, args.num_files)
|
files = get_waymo_scenarios(waymo_data_directory, args.start_file_index, args.num_files)
|
||||||
|
|
||||||
|
logger.info(
|
||||||
|
f"We will read {len(files)} raw files. You set the number of workers to {args.num_workers}. "
|
||||||
|
f"Please make sure there will not be too much files to be read in each worker "
|
||||||
|
f"(now it's {len(files) / args.num_workers})!"
|
||||||
|
)
|
||||||
|
|
||||||
write_to_directory(
|
write_to_directory(
|
||||||
convert_func=convert_waymo_scenario,
|
convert_func=convert_waymo_scenario,
|
||||||
scenarios=files,
|
scenarios=files,
|
||||||
|
|||||||
@@ -224,12 +224,13 @@ def convert_av2_scenario(scenario, version):
|
|||||||
|
|
||||||
# === Waymo specific data. Storing them here ===
|
# === Waymo specific data. Storing them here ===
|
||||||
md_scenario[SD.METADATA]["current_time_index"] = 49
|
md_scenario[SD.METADATA]["current_time_index"] = 49
|
||||||
md_scenario[SD.METADATA]["sdc_track_index"] = scenario.focal_track_id
|
|
||||||
|
|
||||||
# obj id
|
# obj id
|
||||||
obj_keys = list(tracks.keys())
|
obj_keys = list(tracks.keys())
|
||||||
md_scenario[SD.METADATA]["objects_of_interest"] = [obj_keys[idx] for idx, cat in enumerate(category) if cat == 2]
|
md_scenario[SD.METADATA]["objects_of_interest"] = [obj_keys[idx] for idx, cat in enumerate(category) if cat == 2]
|
||||||
|
|
||||||
|
md_scenario[SD.METADATA]["sdc_track_index"] = obj_keys.index('AV')
|
||||||
|
|
||||||
track_index = [obj_keys.index(scenario.focal_track_id)]
|
track_index = [obj_keys.index(scenario.focal_track_id)]
|
||||||
track_id = [scenario.focal_track_id]
|
track_id = [scenario.focal_track_id]
|
||||||
track_difficulty = [0]
|
track_difficulty = [0]
|
||||||
|
|||||||
143
scenarionet/converter/nuplan/block_utils/bfs_roadblock.py
Normal file
143
scenarionet/converter/nuplan/block_utils/bfs_roadblock.py
Normal file
@@ -0,0 +1,143 @@
|
|||||||
|
from collections import deque
|
||||||
|
from typing import Dict, Optional, Tuple, Union, List
|
||||||
|
|
||||||
|
try:
|
||||||
|
from nuplan.common.maps.abstract_map import AbstractMap
|
||||||
|
from nuplan.common.maps.abstract_map_objects import RoadBlockGraphEdgeMapObject
|
||||||
|
except ImportError:
|
||||||
|
AbstractMap = None
|
||||||
|
RoadBlockGraphEdgeMapObject = None
|
||||||
|
|
||||||
|
|
||||||
|
class BreadthFirstSearchRoadBlock:
|
||||||
|
"""
|
||||||
|
A class that performs iterative breadth first search. The class operates on the roadblock graph.
|
||||||
|
"""
|
||||||
|
def __init__(self, start_roadblock_id: int, map_api: Optional[AbstractMap], forward_search: str = True):
|
||||||
|
"""
|
||||||
|
Constructor of BreadthFirstSearchRoadBlock class
|
||||||
|
:param start_roadblock_id: roadblock id where graph starts
|
||||||
|
:param map_api: map class in nuPlan
|
||||||
|
:param forward_search: whether to search in driving direction, defaults to True
|
||||||
|
"""
|
||||||
|
self._map_api: Optional[AbstractMap] = map_api
|
||||||
|
self._queue = deque([self.id_to_roadblock(start_roadblock_id), None])
|
||||||
|
self._parent: Dict[str, Optional[RoadBlockGraphEdgeMapObject]] = dict()
|
||||||
|
self._forward_search = forward_search
|
||||||
|
|
||||||
|
# lazy loaded
|
||||||
|
self._target_roadblock_ids: List[str] = None
|
||||||
|
|
||||||
|
def search(self, target_roadblock_id: Union[str, List[str]],
|
||||||
|
max_depth: int) -> Tuple[List[RoadBlockGraphEdgeMapObject], bool]:
|
||||||
|
"""
|
||||||
|
Apply BFS to find route to target roadblock.
|
||||||
|
:param target_roadblock_id: id of target roadblock
|
||||||
|
:param max_depth: maximum search depth
|
||||||
|
:return: tuple of route and whether a path was found
|
||||||
|
"""
|
||||||
|
|
||||||
|
if isinstance(target_roadblock_id, str):
|
||||||
|
target_roadblock_id = [target_roadblock_id]
|
||||||
|
self._target_roadblock_ids = target_roadblock_id
|
||||||
|
|
||||||
|
start_edge = self._queue[0]
|
||||||
|
|
||||||
|
# Initial search states
|
||||||
|
path_found: bool = False
|
||||||
|
end_edge: RoadBlockGraphEdgeMapObject = start_edge
|
||||||
|
end_depth: int = 1
|
||||||
|
depth: int = 1
|
||||||
|
|
||||||
|
self._parent[start_edge.id + f"_{depth}"] = None
|
||||||
|
|
||||||
|
while self._queue:
|
||||||
|
current_edge = self._queue.popleft()
|
||||||
|
|
||||||
|
# Early exit condition
|
||||||
|
if self._check_end_condition(depth, max_depth):
|
||||||
|
break
|
||||||
|
|
||||||
|
# Depth tracking
|
||||||
|
if current_edge is None:
|
||||||
|
depth += 1
|
||||||
|
self._queue.append(None)
|
||||||
|
if self._queue[0] is None:
|
||||||
|
break
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Goal condition
|
||||||
|
if self._check_goal_condition(current_edge, depth, max_depth):
|
||||||
|
end_edge = current_edge
|
||||||
|
end_depth = depth
|
||||||
|
path_found = True
|
||||||
|
break
|
||||||
|
|
||||||
|
neighbors = (current_edge.outgoing_edges if self._forward_search else current_edge.incoming_edges)
|
||||||
|
|
||||||
|
# Populate queue
|
||||||
|
for next_edge in neighbors:
|
||||||
|
# if next_edge.id in self._candidate_lane_edge_ids_old:
|
||||||
|
self._queue.append(next_edge)
|
||||||
|
self._parent[next_edge.id + f"_{depth + 1}"] = current_edge
|
||||||
|
end_edge = next_edge
|
||||||
|
end_depth = depth + 1
|
||||||
|
|
||||||
|
return self._construct_path(end_edge, end_depth), path_found
|
||||||
|
|
||||||
|
def id_to_roadblock(self, id: str) -> RoadBlockGraphEdgeMapObject:
|
||||||
|
"""
|
||||||
|
Retrieves roadblock from map-api based on id
|
||||||
|
:param id: id of roadblock
|
||||||
|
:return: roadblock class
|
||||||
|
"""
|
||||||
|
block = self._map_api._get_roadblock(id)
|
||||||
|
block = block or self._map_api._get_roadblock_connector(id)
|
||||||
|
return block
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _check_end_condition(depth: int, max_depth: int) -> bool:
|
||||||
|
"""
|
||||||
|
Check if the search should end regardless if the goal condition is met.
|
||||||
|
:param depth: The current depth to check.
|
||||||
|
:param target_depth: The target depth to check against.
|
||||||
|
:return: whether depth exceeds the target depth.
|
||||||
|
"""
|
||||||
|
return depth > max_depth
|
||||||
|
|
||||||
|
def _check_goal_condition(
|
||||||
|
self,
|
||||||
|
current_edge: RoadBlockGraphEdgeMapObject,
|
||||||
|
depth: int,
|
||||||
|
max_depth: int,
|
||||||
|
) -> bool:
|
||||||
|
"""
|
||||||
|
Check if the current edge is at the target roadblock at the given depth.
|
||||||
|
:param current_edge: edge to check.
|
||||||
|
:param depth: current depth to check.
|
||||||
|
:param max_depth: maximum depth the edge should be at.
|
||||||
|
:return: True if the lane edge is contain the in the target roadblock. False, otherwise.
|
||||||
|
"""
|
||||||
|
return current_edge.id in self._target_roadblock_ids and depth <= max_depth
|
||||||
|
|
||||||
|
def _construct_path(self, end_edge: RoadBlockGraphEdgeMapObject, depth: int) -> List[RoadBlockGraphEdgeMapObject]:
|
||||||
|
"""
|
||||||
|
Constructs a path when goal was found.
|
||||||
|
:param end_edge: The end edge to start back propagating back to the start edge.
|
||||||
|
:param depth: The depth of the target edge.
|
||||||
|
:return: The constructed path as a list of RoadBlockGraphEdgeMapObject
|
||||||
|
"""
|
||||||
|
path = [end_edge]
|
||||||
|
path_id = [end_edge.id]
|
||||||
|
|
||||||
|
while self._parent[end_edge.id + f"_{depth}"] is not None:
|
||||||
|
path.append(self._parent[end_edge.id + f"_{depth}"])
|
||||||
|
path_id.append(path[-1].id)
|
||||||
|
end_edge = self._parent[end_edge.id + f"_{depth}"]
|
||||||
|
depth -= 1
|
||||||
|
|
||||||
|
if self._forward_search:
|
||||||
|
path.reverse()
|
||||||
|
path_id.reverse()
|
||||||
|
|
||||||
|
return (path, path_id)
|
||||||
149
scenarionet/converter/nuplan/block_utils/dijkstra.py
Normal file
149
scenarionet/converter/nuplan/block_utils/dijkstra.py
Normal file
@@ -0,0 +1,149 @@
|
|||||||
|
from typing import Dict, List, Optional, Tuple
|
||||||
|
import numpy as np
|
||||||
|
try:
|
||||||
|
from nuplan.common.maps.abstract_map_objects import (
|
||||||
|
LaneGraphEdgeMapObject,
|
||||||
|
RoadBlockGraphEdgeMapObject,
|
||||||
|
)
|
||||||
|
finally:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
class Dijkstra:
|
||||||
|
"""
|
||||||
|
A class that performs dijkstra's shortest path. The class operates on lane level graph search.
|
||||||
|
The goal condition is specified to be if the lane can be found at the target roadblock or roadblock connector.
|
||||||
|
"""
|
||||||
|
def __init__(self, start_edge: LaneGraphEdgeMapObject, candidate_lane_edge_ids: List[str]):
|
||||||
|
"""
|
||||||
|
Constructor for the Dijkstra class.
|
||||||
|
:param start_edge: The starting edge for the search
|
||||||
|
:param candidate_lane_edge_ids: The candidates lane ids that can be included in the search.
|
||||||
|
"""
|
||||||
|
self._queue = list([start_edge])
|
||||||
|
self._parent: Dict[str, Optional[LaneGraphEdgeMapObject]] = dict()
|
||||||
|
self._candidate_lane_edge_ids = candidate_lane_edge_ids
|
||||||
|
|
||||||
|
def search(self, target_roadblock: RoadBlockGraphEdgeMapObject) -> Tuple[List[LaneGraphEdgeMapObject], bool]:
|
||||||
|
"""
|
||||||
|
Performs dijkstra's shortest path to find a route to the target roadblock.
|
||||||
|
:param target_roadblock: The target roadblock the path should end at.
|
||||||
|
:return:
|
||||||
|
- A route starting from the given start edge
|
||||||
|
- A bool indicating if the route is successfully found. Successful means that there exists a path
|
||||||
|
from the start edge to an edge contained in the end roadblock.
|
||||||
|
If unsuccessful the shortest deepest path is returned.
|
||||||
|
"""
|
||||||
|
start_edge = self._queue[0]
|
||||||
|
|
||||||
|
# Initial search states
|
||||||
|
path_found: bool = False
|
||||||
|
end_edge: LaneGraphEdgeMapObject = start_edge
|
||||||
|
|
||||||
|
self._parent[start_edge.id] = None
|
||||||
|
self._frontier = [start_edge.id]
|
||||||
|
self._dist = [1]
|
||||||
|
self._depth = [1]
|
||||||
|
|
||||||
|
self._expanded = []
|
||||||
|
self._expanded_id = []
|
||||||
|
self._expanded_dist = []
|
||||||
|
self._expanded_depth = []
|
||||||
|
|
||||||
|
while len(self._queue) > 0:
|
||||||
|
dist, idx = min((val, idx) for (idx, val) in enumerate(self._dist))
|
||||||
|
current_edge = self._queue[idx]
|
||||||
|
current_depth = self._depth[idx]
|
||||||
|
|
||||||
|
del self._dist[idx], self._queue[idx], self._frontier[idx], self._depth[idx]
|
||||||
|
|
||||||
|
if self._check_goal_condition(current_edge, target_roadblock):
|
||||||
|
end_edge = current_edge
|
||||||
|
path_found = True
|
||||||
|
break
|
||||||
|
|
||||||
|
self._expanded.append(current_edge)
|
||||||
|
self._expanded_id.append(current_edge.id)
|
||||||
|
self._expanded_dist.append(dist)
|
||||||
|
self._expanded_depth.append(current_depth)
|
||||||
|
|
||||||
|
# Populate queue
|
||||||
|
for next_edge in current_edge.outgoing_edges:
|
||||||
|
if not next_edge.id in self._candidate_lane_edge_ids:
|
||||||
|
continue
|
||||||
|
|
||||||
|
alt = dist + self._edge_cost(next_edge)
|
||||||
|
if next_edge.id not in self._expanded_id and next_edge.id not in self._frontier:
|
||||||
|
self._parent[next_edge.id] = current_edge
|
||||||
|
self._queue.append(next_edge)
|
||||||
|
self._frontier.append(next_edge.id)
|
||||||
|
self._dist.append(alt)
|
||||||
|
self._depth.append(current_depth + 1)
|
||||||
|
end_edge = next_edge
|
||||||
|
|
||||||
|
elif next_edge.id in self._frontier:
|
||||||
|
next_edge_idx = self._frontier.index(next_edge.id)
|
||||||
|
current_cost = self._dist[next_edge_idx]
|
||||||
|
if alt < current_cost:
|
||||||
|
self._parent[next_edge.id] = current_edge
|
||||||
|
self._dist[next_edge_idx] = alt
|
||||||
|
self._depth[next_edge_idx] = current_depth + 1
|
||||||
|
|
||||||
|
if not path_found:
|
||||||
|
# filter max depth
|
||||||
|
max_depth = max(self._expanded_depth)
|
||||||
|
idx_max_depth = list(np.where(np.array(self._expanded_depth) == max_depth)[0])
|
||||||
|
dist_at_max_depth = [self._expanded_dist[i] for i in idx_max_depth]
|
||||||
|
|
||||||
|
dist, _idx = min((val, idx) for (idx, val) in enumerate(dist_at_max_depth))
|
||||||
|
end_edge = self._expanded[idx_max_depth[_idx]]
|
||||||
|
|
||||||
|
return self._construct_path(end_edge), path_found
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _edge_cost(lane: LaneGraphEdgeMapObject) -> float:
|
||||||
|
"""
|
||||||
|
Edge cost of given lane.
|
||||||
|
:param lane: lane class
|
||||||
|
:return: length of lane
|
||||||
|
"""
|
||||||
|
return lane.baseline_path.length
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _check_end_condition(depth: int, target_depth: int) -> bool:
|
||||||
|
"""
|
||||||
|
Check if the search should end regardless if the goal condition is met.
|
||||||
|
:param depth: The current depth to check.
|
||||||
|
:param target_depth: The target depth to check against.
|
||||||
|
:return: True if:
|
||||||
|
- The current depth exceeds the target depth.
|
||||||
|
"""
|
||||||
|
return depth > target_depth
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _check_goal_condition(
|
||||||
|
current_edge: LaneGraphEdgeMapObject,
|
||||||
|
target_roadblock: RoadBlockGraphEdgeMapObject,
|
||||||
|
) -> bool:
|
||||||
|
"""
|
||||||
|
Check if the current edge is at the target roadblock at the given depth.
|
||||||
|
:param current_edge: The edge to check.
|
||||||
|
:param target_roadblock: The target roadblock the edge should be contained in.
|
||||||
|
:return: whether the current edge is in the target roadblock
|
||||||
|
"""
|
||||||
|
return current_edge.get_roadblock_id() == target_roadblock.id
|
||||||
|
|
||||||
|
def _construct_path(self, end_edge: LaneGraphEdgeMapObject) -> List[LaneGraphEdgeMapObject]:
|
||||||
|
"""
|
||||||
|
:param end_edge: The end edge to start back propagating back to the start edge.
|
||||||
|
:param depth: The depth of the target edge.
|
||||||
|
:return: The constructed path as a list of LaneGraphEdgeMapObject
|
||||||
|
"""
|
||||||
|
path = [end_edge]
|
||||||
|
while self._parent[end_edge.id] is not None:
|
||||||
|
node = self._parent[end_edge.id]
|
||||||
|
path.append(node)
|
||||||
|
end_edge = node
|
||||||
|
path.reverse()
|
||||||
|
|
||||||
|
return path
|
||||||
230
scenarionet/converter/nuplan/block_utils/route_utils.py
Normal file
230
scenarionet/converter/nuplan/block_utils/route_utils.py
Normal file
@@ -0,0 +1,230 @@
|
|||||||
|
from typing import Dict, List, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
try:
|
||||||
|
from nuplan.common.actor_state.ego_state import EgoState
|
||||||
|
from nuplan.common.actor_state.state_representation import StateSE2
|
||||||
|
from nuplan.common.maps.abstract_map import AbstractMap
|
||||||
|
from nuplan.common.maps.abstract_map_objects import RoadBlockGraphEdgeMapObject
|
||||||
|
from nuplan.common.maps.maps_datatypes import SemanticMapLayer
|
||||||
|
from nuplan.planning.simulation.occupancy_map.strtree_occupancy_map import (
|
||||||
|
STRTreeOccupancyMapFactory,
|
||||||
|
)
|
||||||
|
finally:
|
||||||
|
pass
|
||||||
|
|
||||||
|
from scenarionet.converter.nuplan.block_utils.bfs_roadblock import BreadthFirstSearchRoadBlock
|
||||||
|
|
||||||
|
|
||||||
|
def normalize_angle(angle: float) -> float:
|
||||||
|
return (angle + np.pi) % (2 * np.pi) - np.pi
|
||||||
|
|
||||||
|
|
||||||
|
def get_current_roadblock_candidates(
|
||||||
|
ego_state: EgoState,
|
||||||
|
map_api: AbstractMap,
|
||||||
|
route_roadblocks_dict: Dict[str, RoadBlockGraphEdgeMapObject],
|
||||||
|
heading_error_thresh: float = np.pi / 4,
|
||||||
|
displacement_error_thresh: float = 3,
|
||||||
|
) -> Tuple[RoadBlockGraphEdgeMapObject, List[RoadBlockGraphEdgeMapObject]]:
|
||||||
|
"""
|
||||||
|
Determines a set of roadblock candidate where ego is located
|
||||||
|
:param ego_state: class containing ego state
|
||||||
|
:param map_api: map object
|
||||||
|
:param route_roadblocks_dict: dictionary of on-route roadblocks
|
||||||
|
:param heading_error_thresh: maximum heading error, defaults to np.pi/4
|
||||||
|
:param displacement_error_thresh: maximum displacement, defaults to 3
|
||||||
|
:return: tuple of most promising roadblock and other candidates
|
||||||
|
"""
|
||||||
|
ego_pose: StateSE2 = ego_state.rear_axle
|
||||||
|
roadblock_candidates = []
|
||||||
|
|
||||||
|
layers = [SemanticMapLayer.ROADBLOCK, SemanticMapLayer.ROADBLOCK_CONNECTOR]
|
||||||
|
roadblock_dict = map_api.get_proximal_map_objects(point=ego_pose.point, radius=2.5, layers=layers)
|
||||||
|
roadblock_candidates = (
|
||||||
|
roadblock_dict[SemanticMapLayer.ROADBLOCK] + roadblock_dict[SemanticMapLayer.ROADBLOCK_CONNECTOR]
|
||||||
|
)
|
||||||
|
|
||||||
|
if not roadblock_candidates:
|
||||||
|
for layer in layers:
|
||||||
|
roadblock_id_, distance = map_api.get_distance_to_nearest_map_object(point=ego_pose.point, layer=layer)
|
||||||
|
roadblock = map_api.get_map_object(roadblock_id_, layer)
|
||||||
|
|
||||||
|
if roadblock:
|
||||||
|
roadblock_candidates.append(roadblock)
|
||||||
|
|
||||||
|
on_route_candidates, on_route_candidate_displacement_errors = [], []
|
||||||
|
candidates, candidate_displacement_errors = [], []
|
||||||
|
|
||||||
|
roadblock_displacement_errors = []
|
||||||
|
roadblock_heading_errors = []
|
||||||
|
|
||||||
|
for idx, roadblock in enumerate(roadblock_candidates):
|
||||||
|
lane_displacement_error, lane_heading_error = np.inf, np.inf
|
||||||
|
|
||||||
|
for lane in roadblock.interior_edges:
|
||||||
|
lane_discrete_path: List[StateSE2] = lane.baseline_path.discrete_path
|
||||||
|
lane_discrete_points = np.array([state.point.array for state in lane_discrete_path], dtype=np.float64)
|
||||||
|
lane_state_distances = ((lane_discrete_points - ego_pose.point.array[None, ...])**2.0).sum(axis=-1)**0.5
|
||||||
|
argmin = np.argmin(lane_state_distances)
|
||||||
|
|
||||||
|
heading_error = np.abs(normalize_angle(lane_discrete_path[argmin].heading - ego_pose.heading))
|
||||||
|
displacement_error = lane_state_distances[argmin]
|
||||||
|
|
||||||
|
if displacement_error < lane_displacement_error:
|
||||||
|
lane_heading_error, lane_displacement_error = (
|
||||||
|
heading_error,
|
||||||
|
displacement_error,
|
||||||
|
)
|
||||||
|
|
||||||
|
if (heading_error < heading_error_thresh and displacement_error < displacement_error_thresh):
|
||||||
|
if roadblock.id in route_roadblocks_dict.keys():
|
||||||
|
on_route_candidates.append(roadblock)
|
||||||
|
on_route_candidate_displacement_errors.append(displacement_error)
|
||||||
|
else:
|
||||||
|
candidates.append(roadblock)
|
||||||
|
candidate_displacement_errors.append(displacement_error)
|
||||||
|
|
||||||
|
roadblock_displacement_errors.append(lane_displacement_error)
|
||||||
|
roadblock_heading_errors.append(lane_heading_error)
|
||||||
|
|
||||||
|
if on_route_candidates: # prefer on-route roadblocks
|
||||||
|
return (
|
||||||
|
on_route_candidates[np.argmin(on_route_candidate_displacement_errors)],
|
||||||
|
on_route_candidates,
|
||||||
|
)
|
||||||
|
elif candidates: # fallback to most promising candidate
|
||||||
|
return candidates[np.argmin(candidate_displacement_errors)], candidates
|
||||||
|
|
||||||
|
# otherwise, just find any close roadblock
|
||||||
|
return (
|
||||||
|
roadblock_candidates[np.argmin(roadblock_displacement_errors)],
|
||||||
|
roadblock_candidates,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def route_roadblock_correction(
|
||||||
|
ego_state: EgoState,
|
||||||
|
map_api: AbstractMap,
|
||||||
|
route_roadblock_ids: List[str],
|
||||||
|
search_depth_backward: int = 15,
|
||||||
|
search_depth_forward: int = 30,
|
||||||
|
) -> List[str]:
|
||||||
|
"""
|
||||||
|
Applies several methods to correct route roadblocks.
|
||||||
|
:param ego_state: class containing ego state
|
||||||
|
:param map_api: map object
|
||||||
|
:param route_roadblocks_dict: dictionary of on-route roadblocks
|
||||||
|
:param search_depth_backward: depth of forward BFS search, defaults to 15
|
||||||
|
:param search_depth_forward: depth of backward BFS search, defaults to 30
|
||||||
|
:return: list of roadblock id's of corrected route
|
||||||
|
"""
|
||||||
|
|
||||||
|
route_roadblock_dict = {}
|
||||||
|
for id_ in route_roadblock_ids:
|
||||||
|
block = map_api.get_map_object(id_, SemanticMapLayer.ROADBLOCK)
|
||||||
|
block = block or map_api.get_map_object(id_, SemanticMapLayer.ROADBLOCK_CONNECTOR)
|
||||||
|
route_roadblock_dict[id_] = block
|
||||||
|
|
||||||
|
starting_block, starting_block_candidates = get_current_roadblock_candidates(
|
||||||
|
ego_state, map_api, route_roadblock_dict
|
||||||
|
)
|
||||||
|
starting_block_ids = [roadblock.id for roadblock in starting_block_candidates]
|
||||||
|
|
||||||
|
route_roadblocks = list(route_roadblock_dict.values())
|
||||||
|
route_roadblock_ids = list(route_roadblock_dict.keys())
|
||||||
|
|
||||||
|
# Fix 1: when agent starts off-route
|
||||||
|
if starting_block.id not in route_roadblock_ids:
|
||||||
|
# Backward search if current roadblock not in route
|
||||||
|
graph_search = BreadthFirstSearchRoadBlock(route_roadblock_ids[0], map_api, forward_search=False)
|
||||||
|
(path, path_id), path_found = graph_search.search(starting_block_ids, max_depth=search_depth_backward)
|
||||||
|
|
||||||
|
if path_found:
|
||||||
|
route_roadblocks[:0] = path[:-1]
|
||||||
|
route_roadblock_ids[:0] = path_id[:-1]
|
||||||
|
|
||||||
|
else:
|
||||||
|
# Forward search to any route roadblock
|
||||||
|
graph_search = BreadthFirstSearchRoadBlock(starting_block.id, map_api, forward_search=True)
|
||||||
|
(path, path_id), path_found = graph_search.search(route_roadblock_ids[:3], max_depth=search_depth_forward)
|
||||||
|
|
||||||
|
if path_found:
|
||||||
|
end_roadblock_idx = np.argmax(np.array(route_roadblock_ids) == path_id[-1])
|
||||||
|
|
||||||
|
route_roadblocks = route_roadblocks[end_roadblock_idx + 1:]
|
||||||
|
route_roadblock_ids = route_roadblock_ids[end_roadblock_idx + 1:]
|
||||||
|
|
||||||
|
route_roadblocks[:0] = path
|
||||||
|
route_roadblock_ids[:0] = path_id
|
||||||
|
|
||||||
|
# Fix 2: check if roadblocks are linked, search for links if not
|
||||||
|
roadblocks_to_append = {}
|
||||||
|
for i in range(len(route_roadblocks) - 1):
|
||||||
|
next_incoming_block_ids = [_roadblock.id for _roadblock in route_roadblocks[i + 1].incoming_edges]
|
||||||
|
is_incoming = route_roadblock_ids[i] in next_incoming_block_ids
|
||||||
|
|
||||||
|
if is_incoming:
|
||||||
|
continue
|
||||||
|
|
||||||
|
graph_search = BreadthFirstSearchRoadBlock(route_roadblock_ids[i], map_api, forward_search=True)
|
||||||
|
(path, path_id), path_found = graph_search.search(route_roadblock_ids[i + 1], max_depth=search_depth_forward)
|
||||||
|
|
||||||
|
if path_found and path and len(path) >= 3:
|
||||||
|
path, path_id = path[1:-1], path_id[1:-1]
|
||||||
|
roadblocks_to_append[i] = (path, path_id)
|
||||||
|
|
||||||
|
# append missing intermediate roadblocks
|
||||||
|
offset = 1
|
||||||
|
for i, (path, path_id) in roadblocks_to_append.items():
|
||||||
|
route_roadblocks[i + offset:i + offset] = path
|
||||||
|
route_roadblock_ids[i + offset:i + offset] = path_id
|
||||||
|
offset += len(path)
|
||||||
|
|
||||||
|
# Fix 3: cut route-loops
|
||||||
|
route_roadblocks, route_roadblock_ids = remove_route_loops(route_roadblocks, route_roadblock_ids)
|
||||||
|
|
||||||
|
return route_roadblock_ids
|
||||||
|
|
||||||
|
|
||||||
|
def remove_route_loops(
|
||||||
|
route_roadblocks: List[RoadBlockGraphEdgeMapObject],
|
||||||
|
route_roadblock_ids: List[str],
|
||||||
|
) -> Tuple[List[str], List[RoadBlockGraphEdgeMapObject]]:
|
||||||
|
"""
|
||||||
|
Remove ending of route, if the roadblock are intersecting the route (forming a loop).
|
||||||
|
:param route_roadblocks: input route roadblocks
|
||||||
|
:param route_roadblock_ids: input route roadblocks ids
|
||||||
|
:return: tuple of ids and roadblocks of route without loops
|
||||||
|
"""
|
||||||
|
|
||||||
|
roadblock_occupancy_map = None
|
||||||
|
loop_idx = None
|
||||||
|
|
||||||
|
for idx, roadblock in enumerate(route_roadblocks):
|
||||||
|
# loops only occur at intersection, thus searching for roadblock-connectors.
|
||||||
|
if str(roadblock.__class__.__name__) == "NuPlanRoadBlockConnector":
|
||||||
|
if not roadblock_occupancy_map:
|
||||||
|
roadblock_occupancy_map = STRTreeOccupancyMapFactory.get_from_geometry(
|
||||||
|
[roadblock.polygon], [roadblock.id]
|
||||||
|
)
|
||||||
|
continue
|
||||||
|
|
||||||
|
strtree, index_by_id = roadblock_occupancy_map._build_strtree()
|
||||||
|
indices = strtree.query(roadblock.polygon)
|
||||||
|
if len(indices) > 0:
|
||||||
|
for geom in strtree.geometries.take(indices):
|
||||||
|
area = geom.intersection(roadblock.polygon).area
|
||||||
|
if area > 1:
|
||||||
|
loop_idx = idx
|
||||||
|
break
|
||||||
|
if loop_idx:
|
||||||
|
break
|
||||||
|
|
||||||
|
roadblock_occupancy_map.insert(roadblock.id, roadblock.polygon)
|
||||||
|
|
||||||
|
if loop_idx:
|
||||||
|
route_roadblocks = route_roadblocks[:loop_idx]
|
||||||
|
route_roadblock_ids = route_roadblock_ids[:loop_idx]
|
||||||
|
|
||||||
|
return route_roadblocks, route_roadblock_ids
|
||||||
@@ -4,7 +4,7 @@ import tempfile
|
|||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from os.path import join
|
from os.path import join
|
||||||
from typing import Union
|
from typing import Union
|
||||||
|
from scenarionet.converter.nuplan.block_utils.route_utils import route_roadblock_correction
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from metadrive.scenario import ScenarioDescription as SD
|
from metadrive.scenario import ScenarioDescription as SD
|
||||||
from metadrive.type import MetaDriveType
|
from metadrive.type import MetaDriveType
|
||||||
@@ -69,7 +69,8 @@ def get_nuplan_scenarios(data_root, map_root, logs: Union[list, None] = None, bu
|
|||||||
# filter
|
# filter
|
||||||
"scenario_filter=all_scenarios", # simulate only one log
|
"scenario_filter=all_scenarios", # simulate only one log
|
||||||
"scenario_filter.remove_invalid_goals=true",
|
"scenario_filter.remove_invalid_goals=true",
|
||||||
"scenario_filter.shuffle=true",
|
"scenario_filter.expand_scenarios=false",
|
||||||
|
"scenario_filter.shuffle=false",
|
||||||
"scenario_filter.log_names=[{}]".format(log_string),
|
"scenario_filter.log_names=[{}]".format(log_string),
|
||||||
# "scenario_filter.scenario_types={}".format(all_scenario_types),
|
# "scenario_filter.scenario_types={}".format(all_scenario_types),
|
||||||
# "scenario_filter.scenario_tokens=[]",
|
# "scenario_filter.scenario_tokens=[]",
|
||||||
@@ -78,7 +79,7 @@ def get_nuplan_scenarios(data_root, map_root, logs: Union[list, None] = None, bu
|
|||||||
# "scenario_filter.limit_total_scenarios=1000",
|
# "scenario_filter.limit_total_scenarios=1000",
|
||||||
# "scenario_filter.expand_scenarios=true",
|
# "scenario_filter.expand_scenarios=true",
|
||||||
# "scenario_filter.limit_scenarios_per_type=10", # use 10 scenarios per scenario type
|
# "scenario_filter.limit_scenarios_per_type=10", # use 10 scenarios per scenario type
|
||||||
"scenario_filter.timestamp_threshold_s=20", # minial scenario duration (s)
|
"scenario_filter.timestamp_threshold_s=10", # minial scenario duration (s)
|
||||||
]
|
]
|
||||||
|
|
||||||
base_config_path = os.path.join(nuplan_package_path, "planning", "script")
|
base_config_path = os.path.join(nuplan_package_path, "planning", "script")
|
||||||
@@ -175,7 +176,7 @@ def get_line_type(nuplan_type):
|
|||||||
raise ValueError("Unknown line tyep: {}".format(nuplan_type))
|
raise ValueError("Unknown line tyep: {}".format(nuplan_type))
|
||||||
|
|
||||||
|
|
||||||
def extract_map_features(map_api, center, radius=500):
|
def extract_map_features(map_api, center, route_block_ids, radius=500):
|
||||||
ret = {}
|
ret = {}
|
||||||
np.seterr(all='ignore')
|
np.seterr(all='ignore')
|
||||||
# Center is Important !
|
# Center is Important !
|
||||||
@@ -235,7 +236,9 @@ def extract_map_features(map_api, center, radius=500):
|
|||||||
if layer == SemanticMapLayer.ROADBLOCK else [],
|
if layer == SemanticMapLayer.ROADBLOCK else [],
|
||||||
SD.RIGHT_NEIGHBORS: [edge.id for edge in block.interior_edges[index + 1:]] \
|
SD.RIGHT_NEIGHBORS: [edge.id for edge in block.interior_edges[index + 1:]] \
|
||||||
if layer == SemanticMapLayer.ROADBLOCK else [],
|
if layer == SemanticMapLayer.ROADBLOCK else [],
|
||||||
SD.POLYGON: polygon
|
SD.POLYGON: polygon,
|
||||||
|
"is_sdc_route": lane_meta_data.get_roadblock_id() in route_block_ids,
|
||||||
|
"speed_limit_mps": lane_meta_data.speed_limit_mps,
|
||||||
}
|
}
|
||||||
if layer == SemanticMapLayer.ROADBLOCK_CONNECTOR:
|
if layer == SemanticMapLayer.ROADBLOCK_CONNECTOR:
|
||||||
continue
|
continue
|
||||||
@@ -513,8 +516,15 @@ def convert_nuplan_scenario(scenario: NuPlanScenario, version):
|
|||||||
# traffic light
|
# traffic light
|
||||||
result[SD.DYNAMIC_MAP_STATES] = extract_traffic_light(scenario, scenario_center)
|
result[SD.DYNAMIC_MAP_STATES] = extract_traffic_light(scenario, scenario_center)
|
||||||
|
|
||||||
|
# route
|
||||||
|
route_block_ids = scenario.get_route_roadblock_ids()
|
||||||
|
try:
|
||||||
|
route_block_ids = route_roadblock_correction(state, scenario.map_api, route_block_ids)
|
||||||
|
except Exception as e:
|
||||||
|
logger.error("Route correction failed: {}".format(e))
|
||||||
|
|
||||||
# map
|
# map
|
||||||
result[SD.MAP_FEATURES] = extract_map_features(scenario.map_api, scenario_center)
|
result[SD.MAP_FEATURES] = extract_map_features(scenario.map_api, scenario_center, route_block_ids)
|
||||||
|
|
||||||
return result
|
return result
|
||||||
|
|
||||||
|
|||||||
@@ -256,17 +256,7 @@ def get_tracks_from_frames(nuscenes: NuScenes, scene_info, frames, num_to_interp
|
|||||||
interpolate_tracks[id]["state"][k] = interpolate(v, track["state"]["valid"], new_valid)
|
interpolate_tracks[id]["state"][k] = interpolate(v, track["state"]["valid"], new_valid)
|
||||||
# if id == "ego":
|
# if id == "ego":
|
||||||
# ego is valid all time, so we can calculate the velocity in this way
|
# ego is valid all time, so we can calculate the velocity in this way
|
||||||
|
return interpolate_tracks
|
||||||
# Normalize place all object to (0,0)
|
|
||||||
map_center = np.array(interpolate_tracks["ego"]["state"]["position"][0])
|
|
||||||
map_center[-1] = 0
|
|
||||||
normalized_ret = {}
|
|
||||||
for id, track, in interpolate_tracks.items():
|
|
||||||
pos = track["state"]["position"] - map_center
|
|
||||||
track["state"]["position"] = np.asarray(pos)
|
|
||||||
normalized_ret[id] = track
|
|
||||||
|
|
||||||
return normalized_ret, map_center
|
|
||||||
|
|
||||||
|
|
||||||
def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, points_distance=1, only_lane=False):
|
def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, points_distance=1, only_lane=False):
|
||||||
@@ -323,10 +313,7 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
for idx, boundary in enumerate(boundaries[0]):
|
for idx, boundary in enumerate(boundaries[0]):
|
||||||
block_points = np.array(list(i for i in zip(boundary.coords.xy[0], boundary.coords.xy[1])))
|
block_points = np.array(list(i for i in zip(boundary.coords.xy[0], boundary.coords.xy[1])))
|
||||||
id = "boundary_{}".format(idx)
|
id = "boundary_{}".format(idx)
|
||||||
ret[id] = {
|
ret[id] = {SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_WHITE, SD.POLYLINE: block_points}
|
||||||
SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_WHITE,
|
|
||||||
SD.POLYLINE: block_points - np.asarray(map_center)[:2]
|
|
||||||
}
|
|
||||||
|
|
||||||
# broken line
|
# broken line
|
||||||
for id in map_objs["lane_divider"]:
|
for id in map_objs["lane_divider"]:
|
||||||
@@ -334,7 +321,7 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
assert line_info["token"] == id
|
assert line_info["token"] == id
|
||||||
line = map_api.extract_line(line_info["line_token"]).coords.xy
|
line = map_api.extract_line(line_info["line_token"]).coords.xy
|
||||||
line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))])
|
line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))])
|
||||||
ret[id] = {SD.TYPE: MetaDriveType.LINE_BROKEN_SINGLE_WHITE, SD.POLYLINE: line - np.asarray(map_center)[:2]}
|
ret[id] = {SD.TYPE: MetaDriveType.LINE_BROKEN_SINGLE_WHITE, SD.POLYLINE: line}
|
||||||
|
|
||||||
# solid line
|
# solid line
|
||||||
for id in map_objs["road_divider"]:
|
for id in map_objs["road_divider"]:
|
||||||
@@ -342,7 +329,7 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
assert line_info["token"] == id
|
assert line_info["token"] == id
|
||||||
line = map_api.extract_line(line_info["line_token"]).coords.xy
|
line = map_api.extract_line(line_info["line_token"]).coords.xy
|
||||||
line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))])
|
line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))])
|
||||||
ret[id] = {SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_YELLOW, SD.POLYLINE: line - np.asarray(map_center)[:2]}
|
ret[id] = {SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_YELLOW, SD.POLYLINE: line}
|
||||||
|
|
||||||
# crosswalk
|
# crosswalk
|
||||||
for id in map_objs["ped_crossing"]:
|
for id in map_objs["ped_crossing"]:
|
||||||
@@ -352,7 +339,7 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
||||||
ret[id] = {
|
ret[id] = {
|
||||||
SD.TYPE: MetaDriveType.CROSSWALK,
|
SD.TYPE: MetaDriveType.CROSSWALK,
|
||||||
SD.POLYGON: boundary_polygon - np.asarray(map_center)[:2],
|
SD.POLYGON: boundary_polygon,
|
||||||
}
|
}
|
||||||
|
|
||||||
# walkway
|
# walkway
|
||||||
@@ -363,7 +350,7 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
||||||
ret[id] = {
|
ret[id] = {
|
||||||
SD.TYPE: MetaDriveType.BOUNDARY_SIDEWALK,
|
SD.TYPE: MetaDriveType.BOUNDARY_SIDEWALK,
|
||||||
SD.POLYGON: boundary_polygon - np.asarray(map_center)[:2],
|
SD.POLYGON: boundary_polygon,
|
||||||
}
|
}
|
||||||
|
|
||||||
# normal lane
|
# normal lane
|
||||||
@@ -375,9 +362,8 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
# boundary_polygon += [[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))]
|
# boundary_polygon += [[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))]
|
||||||
ret[id] = {
|
ret[id] = {
|
||||||
SD.TYPE: MetaDriveType.LANE_SURFACE_STREET,
|
SD.TYPE: MetaDriveType.LANE_SURFACE_STREET,
|
||||||
SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)) -
|
SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)),
|
||||||
np.asarray(map_center),
|
SD.POLYGON: boundary_polygon,
|
||||||
SD.POLYGON: boundary_polygon - np.asarray(map_center)[:2],
|
|
||||||
SD.ENTRY: map_api.get_incoming_lane_ids(id),
|
SD.ENTRY: map_api.get_incoming_lane_ids(id),
|
||||||
SD.EXIT: map_api.get_outgoing_lane_ids(id),
|
SD.EXIT: map_api.get_outgoing_lane_ids(id),
|
||||||
SD.LEFT_NEIGHBORS: [],
|
SD.LEFT_NEIGHBORS: [],
|
||||||
@@ -393,8 +379,7 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
# boundary_polygon += [[boundary[0][i], boundary[1][i], 0.] for i in range(len(boundary[0]))]
|
# boundary_polygon += [[boundary[0][i], boundary[1][i], 0.] for i in range(len(boundary[0]))]
|
||||||
ret[id] = {
|
ret[id] = {
|
||||||
SD.TYPE: MetaDriveType.LANE_SURFACE_UNSTRUCTURE,
|
SD.TYPE: MetaDriveType.LANE_SURFACE_UNSTRUCTURE,
|
||||||
SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)) -
|
SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)),
|
||||||
np.asarray(map_center),
|
|
||||||
# SD.POLYGON: boundary_polygon,
|
# SD.POLYGON: boundary_polygon,
|
||||||
"speed_limit_kmh": 100,
|
"speed_limit_kmh": 100,
|
||||||
SD.ENTRY: map_api.get_incoming_lane_ids(id),
|
SD.ENTRY: map_api.get_incoming_lane_ids(id),
|
||||||
@@ -409,7 +394,7 @@ def get_map_features(scene_info, nuscenes: NuScenes, map_center, radius=500, poi
|
|||||||
# boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
# boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
||||||
# ret[id] = {
|
# ret[id] = {
|
||||||
# SD.TYPE: MetaDriveType.STOP_LINE,
|
# SD.TYPE: MetaDriveType.STOP_LINE,
|
||||||
# SD.POLYGON: boundary_polygon - np.asarray(map_center)[:2],
|
# SD.POLYGON: boundary_polygon ,
|
||||||
# }
|
# }
|
||||||
|
|
||||||
# 'stop_line',
|
# 'stop_line',
|
||||||
@@ -469,32 +454,25 @@ def convert_nuscenes_scenario(
|
|||||||
result[SD.METADATA]["sample_rate"] = scenario_log_interval
|
result[SD.METADATA]["sample_rate"] = scenario_log_interval
|
||||||
result[SD.METADATA][SD.TIMESTEP] = np.arange(0., (len(frames) - 1) * 0.5 + 0.1, 0.1)
|
result[SD.METADATA][SD.TIMESTEP] = np.arange(0., (len(frames) - 1) * 0.5 + 0.1, 0.1)
|
||||||
# interpolating to 0.1s interval
|
# interpolating to 0.1s interval
|
||||||
result[SD.TRACKS], map_center = get_tracks_from_frames(nuscenes, scene_info, frames, num_to_interpolate=5)
|
result[SD.TRACKS] = get_tracks_from_frames(nuscenes, scene_info, frames, num_to_interpolate=5)
|
||||||
result[SD.METADATA][SD.SDC_ID] = "ego"
|
result[SD.METADATA][SD.SDC_ID] = "ego"
|
||||||
|
|
||||||
# No traffic light in nuscenes at this stage
|
# No traffic light in nuscenes at this stage
|
||||||
result[SD.DYNAMIC_MAP_STATES] = {}
|
result[SD.DYNAMIC_MAP_STATES] = {}
|
||||||
# track_to_predict = result[SD.TRACKS][instance_token]
|
if prediction:
|
||||||
# result[SD.METADATA]["tracks_to_predict"] = {
|
track_to_predict = result[SD.TRACKS][instance_token]
|
||||||
# instance_token: {
|
result[SD.METADATA]["tracks_to_predict"] = {
|
||||||
# "track_index": list(result[SD.TRACKS].keys()).index(instance_token),
|
instance_token: {
|
||||||
# "track_id": instance_token,
|
"track_index": list(result[SD.TRACKS].keys()).index(instance_token),
|
||||||
# "difficulty": 0,
|
"track_id": instance_token,
|
||||||
# "object_type": track_to_predict['type']
|
"difficulty": 0,
|
||||||
# }
|
"object_type": track_to_predict['type']
|
||||||
# }
|
}
|
||||||
# map
|
}
|
||||||
result[SD.MAP_FEATURES] = get_map_features(scene_info, nuscenes, map_center, map_radius, only_lane=only_lane)
|
|
||||||
# add back map center
|
|
||||||
map_center = map_center[np.newaxis]
|
|
||||||
for k, v in result[SD.TRACKS].items():
|
|
||||||
v['state']['position'] += map_center
|
|
||||||
|
|
||||||
for k, v in result[SD.MAP_FEATURES].items():
|
# map
|
||||||
if 'polygon' in v:
|
map_center = np.array(result[SD.TRACKS]["ego"]["state"]["position"][0])
|
||||||
v['polygon'] += map_center[:, :v['polygon'].shape[-1]]
|
result[SD.MAP_FEATURES] = get_map_features(scene_info, nuscenes, map_center, map_radius, only_lane=only_lane)
|
||||||
else:
|
|
||||||
v['polyline'] += map_center[:, :v['polyline'].shape[-1]]
|
|
||||||
del frames_scene_info
|
del frames_scene_info
|
||||||
del frames
|
del frames
|
||||||
del scene_info
|
del scene_info
|
||||||
|
|||||||
@@ -15,7 +15,7 @@ import tqdm
|
|||||||
from metadrive.scenario import ScenarioDescription as SD
|
from metadrive.scenario import ScenarioDescription as SD
|
||||||
|
|
||||||
from scenarionet.builder.utils import merge_database
|
from scenarionet.builder.utils import merge_database
|
||||||
from scenarionet.common_utils import save_summary_anda_mapping
|
from scenarionet.common_utils import save_summary_and_mapping
|
||||||
from scenarionet.converter.pg.utils import convert_pg_scenario, make_env
|
from scenarionet.converter.pg.utils import convert_pg_scenario, make_env
|
||||||
|
|
||||||
logger = logging.getLogger(__file__)
|
logger = logging.getLogger(__file__)
|
||||||
@@ -218,7 +218,8 @@ def write_to_directory_single_worker(
|
|||||||
kwargs["env"] = make_env(start_index=scenarios[0], num_scenarios=len(scenarios))
|
kwargs["env"] = make_env(start_index=scenarios[0], num_scenarios=len(scenarios))
|
||||||
|
|
||||||
count = 0
|
count = 0
|
||||||
for scenario in tqdm.tqdm(scenarios, desc="Worker Index: {}".format(worker_index)):
|
# for scenario in tqdm.tqdm(scenarios, position=2, leave=True, desc=f"Worker {worker_index} Number of scenarios"):
|
||||||
|
for scenario in scenarios:
|
||||||
# convert scenario
|
# convert scenario
|
||||||
sd_scenario = convert_func(scenario, dataset_version, **kwargs)
|
sd_scenario = convert_func(scenario, dataset_version, **kwargs)
|
||||||
scenario_id = sd_scenario[SD.ID]
|
scenario_id = sd_scenario[SD.ID]
|
||||||
@@ -248,8 +249,11 @@ def write_to_directory_single_worker(
|
|||||||
print("Current Memory: {}".format(process_memory()))
|
print("Current Memory: {}".format(process_memory()))
|
||||||
count += 1
|
count += 1
|
||||||
|
|
||||||
|
if count % 500 == 0:
|
||||||
|
logger.info(f"Worker {worker_index} has processed {count} scenarios.")
|
||||||
|
|
||||||
# store summary file
|
# store summary file
|
||||||
save_summary_anda_mapping(summary_file_path, mapping_file_path, summary, mapping)
|
save_summary_and_mapping(summary_file_path, mapping_file_path, summary, mapping)
|
||||||
|
|
||||||
# rename and save
|
# rename and save
|
||||||
if delay_remove is not None:
|
if delay_remove is not None:
|
||||||
@@ -257,6 +261,8 @@ def write_to_directory_single_worker(
|
|||||||
shutil.rmtree(delay_remove)
|
shutil.rmtree(delay_remove)
|
||||||
os.rename(output_path, save_path)
|
os.rename(output_path, save_path)
|
||||||
|
|
||||||
|
logger.info(f"Worker {worker_index} finished! Files are saved at: {save_path}")
|
||||||
|
|
||||||
|
|
||||||
def process_memory():
|
def process_memory():
|
||||||
process = psutil.Process(os.getpid())
|
process = psutil.Process(os.getpid())
|
||||||
|
|||||||
0
scenarionet/converter/vod/__init__.py
Normal file
0
scenarionet/converter/vod/__init__.py
Normal file
90
scenarionet/converter/vod/type.py
Normal file
90
scenarionet/converter/vod/type.py
Normal file
@@ -0,0 +1,90 @@
|
|||||||
|
ALL_TYPE = {
|
||||||
|
"noise": 'noise',
|
||||||
|
"human.pedestrian.adult": 'adult',
|
||||||
|
"human.pedestrian.child": 'child',
|
||||||
|
"human.pedestrian.wheelchair": 'wheelchair',
|
||||||
|
"human.pedestrian.stroller": 'stroller',
|
||||||
|
"human.pedestrian.personal_mobility": 'p.mobility',
|
||||||
|
"human.pedestrian.police_officer": 'police',
|
||||||
|
"human.pedestrian.construction_worker": 'worker',
|
||||||
|
"animal": 'animal',
|
||||||
|
"vehicle.car": 'car',
|
||||||
|
"vehicle.motorcycle": 'motorcycle',
|
||||||
|
"vehicle.bicycle": 'bicycle',
|
||||||
|
"vehicle.bus.bendy": 'bus.bendy',
|
||||||
|
"vehicle.bus.rigid": 'bus.rigid',
|
||||||
|
"vehicle.truck": 'truck',
|
||||||
|
"vehicle.construction": 'constr. veh',
|
||||||
|
"vehicle.emergency.ambulance": 'ambulance',
|
||||||
|
"vehicle.emergency.police": 'police car',
|
||||||
|
"vehicle.trailer": 'trailer',
|
||||||
|
"movable_object.barrier": 'barrier',
|
||||||
|
"movable_object.trafficcone": 'trafficcone',
|
||||||
|
"movable_object.pushable_pullable": 'push/pullable',
|
||||||
|
"movable_object.debris": 'debris',
|
||||||
|
"static_object.bicycle_rack": 'bicycle racks',
|
||||||
|
"flat.driveable_surface": 'driveable',
|
||||||
|
"flat.sidewalk": 'sidewalk',
|
||||||
|
"flat.terrain": 'terrain',
|
||||||
|
"flat.other": 'flat.other',
|
||||||
|
"static.manmade": 'manmade',
|
||||||
|
"static.vegetation": 'vegetation',
|
||||||
|
"static.other": 'static.other',
|
||||||
|
"vehicle.ego": "ego",
|
||||||
|
# ADDED:
|
||||||
|
"static.vehicle.bicycle": "static.other",
|
||||||
|
"static.vehicle.motorcycle": "static.other",
|
||||||
|
"vehicle.other": "vehicle.other",
|
||||||
|
"static.vehicle.other": "static.other",
|
||||||
|
"vehicle.unknown": "vehicle.unknown"
|
||||||
|
}
|
||||||
|
NOISE_TYPE = {
|
||||||
|
"noise": 'noise',
|
||||||
|
"animal": 'animal',
|
||||||
|
"static_object.bicycle_rack": 'bicycle racks',
|
||||||
|
"movable_object.pushable_pullable": 'push/pullable',
|
||||||
|
"movable_object.debris": 'debris',
|
||||||
|
"static.manmade": 'manmade',
|
||||||
|
"static.vegetation": 'vegetation',
|
||||||
|
"static.other": 'static.other',
|
||||||
|
"static.vehicle.bicycle": "static.other",
|
||||||
|
"static.vehicle.motorcycle": "static.other",
|
||||||
|
"static.vehicle.other": "static.other",
|
||||||
|
}
|
||||||
|
HUMAN_TYPE = {
|
||||||
|
"human.pedestrian.adult": 'adult',
|
||||||
|
"human.pedestrian.child": 'child',
|
||||||
|
"human.pedestrian.wheelchair": 'wheelchair',
|
||||||
|
"human.pedestrian.stroller": 'stroller',
|
||||||
|
"human.pedestrian.personal_mobility": 'p.mobility',
|
||||||
|
"human.pedestrian.police_officer": 'police',
|
||||||
|
"human.pedestrian.construction_worker": 'worker',
|
||||||
|
}
|
||||||
|
BICYCLE_TYPE = {
|
||||||
|
"vehicle.bicycle": 'bicycle',
|
||||||
|
"vehicle.motorcycle": 'motorcycle',
|
||||||
|
}
|
||||||
|
VEHICLE_TYPE = {
|
||||||
|
"vehicle.car": 'car',
|
||||||
|
"vehicle.bus.bendy": 'bus.bendy',
|
||||||
|
"vehicle.bus.rigid": 'bus.rigid',
|
||||||
|
"vehicle.truck": 'truck',
|
||||||
|
"vehicle.construction": 'constr. veh',
|
||||||
|
"vehicle.emergency.ambulance": 'ambulance',
|
||||||
|
"vehicle.emergency.police": 'police car',
|
||||||
|
"vehicle.trailer": 'trailer',
|
||||||
|
"vehicle.ego": "ego",
|
||||||
|
# ADDED:
|
||||||
|
"vehicle.other": "vehicle.other",
|
||||||
|
"vehicle.unknown": "vehicle.other"
|
||||||
|
}
|
||||||
|
OBSTACLE_TYPE = {
|
||||||
|
"movable_object.barrier": 'barrier',
|
||||||
|
"movable_object.trafficcone": 'trafficcone',
|
||||||
|
}
|
||||||
|
TERRAIN_TYPE = {
|
||||||
|
"flat.driveable_surface": 'driveable',
|
||||||
|
"flat.sidewalk": 'sidewalk',
|
||||||
|
"flat.terrain": 'terrain',
|
||||||
|
"flat.other": 'flat.other'
|
||||||
|
}
|
||||||
558
scenarionet/converter/vod/utils.py
Normal file
558
scenarionet/converter/vod/utils.py
Normal file
@@ -0,0 +1,558 @@
|
|||||||
|
import copy
|
||||||
|
import logging
|
||||||
|
|
||||||
|
import geopandas as gpd
|
||||||
|
import numpy as np
|
||||||
|
from metadrive.scenario import ScenarioDescription as SD
|
||||||
|
from metadrive.type import MetaDriveType
|
||||||
|
from vod.eval.prediction.splits import get_prediction_challenge_split
|
||||||
|
from shapely.ops import unary_union
|
||||||
|
|
||||||
|
from scenarionet.converter.vod.type import (
|
||||||
|
ALL_TYPE,
|
||||||
|
HUMAN_TYPE,
|
||||||
|
BICYCLE_TYPE,
|
||||||
|
VEHICLE_TYPE,
|
||||||
|
)
|
||||||
|
|
||||||
|
logger = logging.getLogger(__name__)
|
||||||
|
try:
|
||||||
|
import logging
|
||||||
|
|
||||||
|
logging.getLogger("shapely.geos").setLevel(logging.CRITICAL)
|
||||||
|
from vod import VOD
|
||||||
|
from vod.can_bus.can_bus_api import VODCanBus
|
||||||
|
from vod.eval.common.utils import quaternion_yaw
|
||||||
|
from vod.map_expansion.arcline_path_utils import discretize_lane
|
||||||
|
from vod.map_expansion.map_api import VODMap
|
||||||
|
from pyquaternion import Quaternion
|
||||||
|
except ImportError as e:
|
||||||
|
logger.warning("Can not import vod-devkit: {}".format(e))
|
||||||
|
|
||||||
|
EGO = "ego"
|
||||||
|
|
||||||
|
|
||||||
|
def get_metadrive_type(obj_type):
|
||||||
|
meta_type = obj_type
|
||||||
|
md_type = None
|
||||||
|
if ALL_TYPE[obj_type] == "barrier":
|
||||||
|
md_type = MetaDriveType.TRAFFIC_BARRIER
|
||||||
|
elif ALL_TYPE[obj_type] == "trafficcone":
|
||||||
|
md_type = MetaDriveType.TRAFFIC_CONE
|
||||||
|
elif obj_type in VEHICLE_TYPE:
|
||||||
|
md_type = MetaDriveType.VEHICLE
|
||||||
|
elif obj_type in HUMAN_TYPE:
|
||||||
|
md_type = MetaDriveType.PEDESTRIAN
|
||||||
|
elif obj_type in BICYCLE_TYPE:
|
||||||
|
md_type = MetaDriveType.CYCLIST
|
||||||
|
|
||||||
|
# assert meta_type != MetaDriveType.UNSET and meta_type != "noise"
|
||||||
|
return md_type, meta_type
|
||||||
|
|
||||||
|
|
||||||
|
def parse_frame(frame, vod: VOD):
|
||||||
|
ret = {}
|
||||||
|
for obj_id in frame["anns"]:
|
||||||
|
obj = vod.get("sample_annotation", obj_id)
|
||||||
|
# velocity = vod.box_velocity(obj_id)[:2]
|
||||||
|
# if np.nan in velocity:
|
||||||
|
velocity = np.array([0.0, 0.0])
|
||||||
|
ret[obj["instance_token"]] = {
|
||||||
|
"position": obj["translation"],
|
||||||
|
"obj_id": obj["instance_token"],
|
||||||
|
"heading": quaternion_yaw(Quaternion(*obj["rotation"])),
|
||||||
|
"rotation": obj["rotation"],
|
||||||
|
"velocity": velocity,
|
||||||
|
"size": obj["size"],
|
||||||
|
"visible": obj["visibility_token"],
|
||||||
|
"attribute": [vod.get("attribute", i)["name"] for i in obj["attribute_tokens"]],
|
||||||
|
"type": obj["category_name"],
|
||||||
|
}
|
||||||
|
# print(frame["data"]["dummy"])
|
||||||
|
ego_token = vod.get("sample_data", frame["data"]["dummy"])["ego_pose_token"]
|
||||||
|
# print(ego_token)
|
||||||
|
ego_state = vod.get("ego_pose", ego_token)
|
||||||
|
ret[EGO] = {
|
||||||
|
"position": ego_state["translation"],
|
||||||
|
"obj_id": EGO,
|
||||||
|
"heading": quaternion_yaw(Quaternion(*ego_state["rotation"])),
|
||||||
|
"rotation": ego_state["rotation"],
|
||||||
|
"type": "vehicle.car",
|
||||||
|
"velocity": np.array([0.0, 0.0]),
|
||||||
|
# size https://en.wikipedia.org/wiki/Renault_Zoe
|
||||||
|
"size": [4.08, 1.73, 1.56],
|
||||||
|
}
|
||||||
|
return ret
|
||||||
|
|
||||||
|
|
||||||
|
def interpolate_heading(heading_data, old_valid, new_valid, num_to_interpolate=1):
|
||||||
|
new_heading_theta = np.zeros_like(new_valid)
|
||||||
|
for k, valid in enumerate(old_valid[:-1]):
|
||||||
|
if abs(valid) > 1e-1 and abs(old_valid[k + 1]) > 1e-1:
|
||||||
|
diff = (heading_data[k + 1] - heading_data[k] + np.pi) % (2 * np.pi) - np.pi
|
||||||
|
# step = diff
|
||||||
|
interpolate_heading = np.linspace(heading_data[k], heading_data[k] + diff, 2) # not sure if 2 is correct
|
||||||
|
new_heading_theta[k * num_to_interpolate:(k + 1) * num_to_interpolate] = (interpolate_heading[:-1])
|
||||||
|
elif abs(valid) > 1e-1 and abs(old_valid[k + 1]) < 1e-1:
|
||||||
|
new_heading_theta[k * num_to_interpolate:(k + 1) * num_to_interpolate] = (heading_data[k])
|
||||||
|
new_heading_theta[-1] = heading_data[-1]
|
||||||
|
return new_heading_theta * new_valid
|
||||||
|
|
||||||
|
|
||||||
|
def _interpolate_one_dim(data, old_valid, new_valid, num_to_interpolate=1):
|
||||||
|
new_data = np.zeros_like(new_valid)
|
||||||
|
for k, valid in enumerate(old_valid[:-1]):
|
||||||
|
if abs(valid) > 1e-1 and abs(old_valid[k + 1]) > 1e-1:
|
||||||
|
diff = data[k + 1] - data[k]
|
||||||
|
# step = diff
|
||||||
|
interpolate_data = np.linspace(data[k], data[k] + diff, num_to_interpolate + 1)
|
||||||
|
new_data[k * num_to_interpolate:(k + 1) * num_to_interpolate] = (interpolate_data[:-1])
|
||||||
|
elif abs(valid) > 1e-1 and abs(old_valid[k + 1]) < 1e-1:
|
||||||
|
new_data[k * num_to_interpolate:(k + 1) * num_to_interpolate] = data[k]
|
||||||
|
new_data[-1] = data[-1]
|
||||||
|
return new_data * new_valid
|
||||||
|
|
||||||
|
|
||||||
|
def interpolate(origin_y, valid, new_valid):
|
||||||
|
if len(origin_y.shape) == 1:
|
||||||
|
ret = _interpolate_one_dim(origin_y, valid, new_valid)
|
||||||
|
elif len(origin_y.shape) == 2:
|
||||||
|
ret = []
|
||||||
|
for dim in range(origin_y.shape[-1]):
|
||||||
|
new_y = _interpolate_one_dim(origin_y[..., dim], valid, new_valid)
|
||||||
|
new_y = np.expand_dims(new_y, axis=-1)
|
||||||
|
ret.append(new_y)
|
||||||
|
ret = np.concatenate(ret, axis=-1)
|
||||||
|
else:
|
||||||
|
raise ValueError("Y has shape {}, Can not interpolate".format(origin_y.shape))
|
||||||
|
return ret
|
||||||
|
|
||||||
|
|
||||||
|
def get_tracks_from_frames(vod: VOD, scene_info, frames, num_to_interpolate=5):
|
||||||
|
episode_len = len(frames)
|
||||||
|
# Fill tracks
|
||||||
|
all_objs = set()
|
||||||
|
for frame in frames:
|
||||||
|
all_objs.update(frame.keys())
|
||||||
|
tracks = {
|
||||||
|
k: dict(
|
||||||
|
type=MetaDriveType.UNSET,
|
||||||
|
state=dict(
|
||||||
|
position=np.zeros(shape=(episode_len, 3)),
|
||||||
|
heading=np.zeros(shape=(episode_len, )),
|
||||||
|
velocity=np.zeros(shape=(episode_len, 2)),
|
||||||
|
valid=np.zeros(shape=(episode_len, )),
|
||||||
|
length=np.zeros(shape=(episode_len, 1)),
|
||||||
|
width=np.zeros(shape=(episode_len, 1)),
|
||||||
|
height=np.zeros(shape=(episode_len, 1)),
|
||||||
|
),
|
||||||
|
metadata=dict(
|
||||||
|
track_length=episode_len,
|
||||||
|
type=MetaDriveType.UNSET,
|
||||||
|
object_id=k,
|
||||||
|
original_id=k,
|
||||||
|
),
|
||||||
|
)
|
||||||
|
for k in list(all_objs)
|
||||||
|
}
|
||||||
|
|
||||||
|
tracks_to_remove = set()
|
||||||
|
first = True
|
||||||
|
a = 0
|
||||||
|
for frame_idx in range(episode_len):
|
||||||
|
# Record all agents' states (position, velocity, ...)
|
||||||
|
# if frame_idx == 0:
|
||||||
|
# continue
|
||||||
|
for id, state in frames[frame_idx].items():
|
||||||
|
# Fill type
|
||||||
|
md_type, meta_type = get_metadrive_type(state["type"])
|
||||||
|
tracks[id]["type"] = md_type
|
||||||
|
tracks[id][SD.METADATA]["type"] = meta_type
|
||||||
|
if md_type is None or md_type == MetaDriveType.UNSET:
|
||||||
|
tracks_to_remove.add(id)
|
||||||
|
continue
|
||||||
|
elif first:
|
||||||
|
first = False
|
||||||
|
id_f = id
|
||||||
|
|
||||||
|
if id == id_f:
|
||||||
|
a += 1
|
||||||
|
# print("FOOUND KEY: ", a, episode_len)
|
||||||
|
# print(state["position"])
|
||||||
|
tracks[id]["type"] = md_type
|
||||||
|
tracks[id][SD.METADATA]["type"] = meta_type
|
||||||
|
|
||||||
|
# Introducing the state item
|
||||||
|
if ((frame_idx == 0) or (frame_idx == 1)) and (id == list(frames[frame_idx].keys())[0]):
|
||||||
|
if state["position"][0] != 0:
|
||||||
|
print(state["position"], md_type)
|
||||||
|
tracks[id]["state"]["position"][frame_idx] = state["position"]
|
||||||
|
tracks[id]["state"]["heading"][frame_idx] = state["heading"]
|
||||||
|
tracks[id]["state"]["velocity"][frame_idx] = tracks[id]["state"]["velocity"][frame_idx]
|
||||||
|
tracks[id]["state"]["valid"][frame_idx] = 1
|
||||||
|
|
||||||
|
tracks[id]["state"]["length"][frame_idx] = state["size"][1]
|
||||||
|
tracks[id]["state"]["width"][frame_idx] = state["size"][0]
|
||||||
|
tracks[id]["state"]["height"][frame_idx] = state["size"][2]
|
||||||
|
|
||||||
|
tracks[id]["metadata"]["original_id"] = id
|
||||||
|
tracks[id]["metadata"]["object_id"] = id
|
||||||
|
|
||||||
|
for track in tracks_to_remove:
|
||||||
|
track_data = tracks.pop(track)
|
||||||
|
obj_type = track_data[SD.METADATA]["type"]
|
||||||
|
print("\nWARNING: Can not map type: {} to any MetaDrive Type".format(obj_type))
|
||||||
|
|
||||||
|
new_episode_len = (episode_len - 1) * num_to_interpolate + 1
|
||||||
|
|
||||||
|
# interpolate
|
||||||
|
interpolate_tracks = {}
|
||||||
|
for (
|
||||||
|
id,
|
||||||
|
track,
|
||||||
|
) in tracks.items():
|
||||||
|
interpolate_tracks[id] = copy.deepcopy(track)
|
||||||
|
interpolate_tracks[id]["metadata"]["track_length"] = new_episode_len
|
||||||
|
|
||||||
|
# valid first
|
||||||
|
new_valid = np.zeros(shape=(new_episode_len, ))
|
||||||
|
if track["state"]["valid"][0]:
|
||||||
|
new_valid[0] = 1
|
||||||
|
for k, valid in enumerate(track["state"]["valid"][1:], start=1):
|
||||||
|
if valid:
|
||||||
|
if abs(new_valid[(k - 1) * num_to_interpolate] - 1) < 1e-2:
|
||||||
|
start_idx = (k - 1) * num_to_interpolate + 1
|
||||||
|
else:
|
||||||
|
start_idx = k * num_to_interpolate
|
||||||
|
new_valid[start_idx:k * num_to_interpolate + 1] = 1
|
||||||
|
interpolate_tracks[id]["state"]["valid"] = new_valid
|
||||||
|
|
||||||
|
# position
|
||||||
|
interpolate_tracks[id]["state"]["position"] = interpolate(
|
||||||
|
track["state"]["position"], track["state"]["valid"], new_valid
|
||||||
|
)
|
||||||
|
# print(np.diff(track["state"]["position"], axis=0))
|
||||||
|
# print(interpolate_tracks[id]["state"]["position"], track["state"]["position"])
|
||||||
|
if id == "ego" and not scene_info.get("prediction", False):
|
||||||
|
assert "prediction" not in scene_info
|
||||||
|
# We can get it from canbus
|
||||||
|
try:
|
||||||
|
canbus = VODCanBus(dataroot=vod.dataroot)
|
||||||
|
imu_pos = np.asarray([state["pos"] for state in canbus.get_messages(scene_info["name"], "pose")[::5]])
|
||||||
|
min_len = min(len(imu_pos), new_episode_len)
|
||||||
|
interpolate_tracks[id]["state"]["position"][:min_len] = imu_pos[:min_len]
|
||||||
|
except:
|
||||||
|
logger.info("Fail to get canbus data for {}".format(scene_info["name"]))
|
||||||
|
|
||||||
|
# velocity
|
||||||
|
interpolate_tracks[id]["state"]["velocity"] = interpolate(
|
||||||
|
track["state"]["velocity"], track["state"]["valid"], new_valid
|
||||||
|
)
|
||||||
|
vel = (interpolate_tracks[id]["state"]["position"][1:] - interpolate_tracks[id]["state"]["position"][:-1])
|
||||||
|
interpolate_tracks[id]["state"]["velocity"][:-1] = vel[..., :2] / 0.1
|
||||||
|
for k, valid in enumerate(new_valid[1:], start=1):
|
||||||
|
if valid == 0 or not valid or abs(valid) < 1e-2:
|
||||||
|
interpolate_tracks[id]["state"]["velocity"][k] = np.array([0.0, 0.0])
|
||||||
|
interpolate_tracks[id]["state"]["velocity"][k - 1] = np.array([0.0, 0.0])
|
||||||
|
# speed outlier check
|
||||||
|
max_vel = np.max(np.linalg.norm(interpolate_tracks[id]["state"]["velocity"], axis=-1))
|
||||||
|
if max_vel > 30:
|
||||||
|
print("\nWARNING: Too large speed for {}: {}".format(id, max_vel))
|
||||||
|
|
||||||
|
# heading
|
||||||
|
# then update position
|
||||||
|
new_heading = interpolate_heading(track["state"]["heading"], track["state"]["valid"], new_valid)
|
||||||
|
interpolate_tracks[id]["state"]["heading"] = new_heading
|
||||||
|
if id == "ego" and not scene_info.get("prediction", False):
|
||||||
|
assert "prediction" not in scene_info
|
||||||
|
# We can get it from canbus
|
||||||
|
try:
|
||||||
|
canbus = VODCanBus(dataroot=vod.dataroot)
|
||||||
|
imu_heading = np.asarray(
|
||||||
|
[
|
||||||
|
quaternion_yaw(Quaternion(state["orientation"]))
|
||||||
|
for state in canbus.get_messages(scene_info["name"], "pose")[::5]
|
||||||
|
]
|
||||||
|
)
|
||||||
|
min_len = min(len(imu_heading), new_episode_len)
|
||||||
|
interpolate_tracks[id]["state"]["heading"][:min_len] = imu_heading[:min_len]
|
||||||
|
except:
|
||||||
|
logger.info("Fail to get canbus data for {}".format(scene_info["name"]))
|
||||||
|
|
||||||
|
for k, v in track["state"].items():
|
||||||
|
if k in ["valid", "heading", "position", "velocity"]:
|
||||||
|
continue
|
||||||
|
else:
|
||||||
|
interpolate_tracks[id]["state"][k] = interpolate(v, track["state"]["valid"], new_valid)
|
||||||
|
# if id == "ego":
|
||||||
|
# ego is valid all time, so we can calculate the velocity in this way
|
||||||
|
return interpolate_tracks
|
||||||
|
|
||||||
|
|
||||||
|
def get_map_features(scene_info, vod: VOD, map_center, radius=500, points_distance=1, only_lane=False):
|
||||||
|
"""
|
||||||
|
Extract map features from vod data. The objects in specified region will be returned. Sampling rate determines
|
||||||
|
the distance between 2 points when extracting lane center line.
|
||||||
|
"""
|
||||||
|
ret = {}
|
||||||
|
map_name = vod.get("log", scene_info["log_token"])["location"]
|
||||||
|
map_api = VODMap(dataroot=vod.dataroot, map_name=map_name)
|
||||||
|
|
||||||
|
layer_names = [
|
||||||
|
# "line",
|
||||||
|
# "polygon",
|
||||||
|
# "node",
|
||||||
|
"drivable_area",
|
||||||
|
"road_segment",
|
||||||
|
# 'road_block',
|
||||||
|
"lane",
|
||||||
|
"ped_crossing",
|
||||||
|
"walkway",
|
||||||
|
# 'stop_line',
|
||||||
|
# 'carpark_area',
|
||||||
|
"lane_connector",
|
||||||
|
# 'road_divider',
|
||||||
|
# 'lane_divider',
|
||||||
|
# 'traffic_light'
|
||||||
|
]
|
||||||
|
# road segment includes all roadblocks (a list of lanes in the same direction), intersection and unstructured road
|
||||||
|
|
||||||
|
map_objs = map_api.get_records_in_radius(map_center[0], map_center[1], radius, layer_names)
|
||||||
|
|
||||||
|
if not only_lane:
|
||||||
|
# build map boundary
|
||||||
|
polygons = []
|
||||||
|
for id in map_objs["drivable_area"]:
|
||||||
|
seg_info = map_api.get("drivable_area", id)
|
||||||
|
assert seg_info["token"] == id
|
||||||
|
for polygon_token in seg_info["polygon_tokens"]:
|
||||||
|
polygon = map_api.extract_polygon(polygon_token)
|
||||||
|
polygons.append(polygon)
|
||||||
|
# for id in map_objs["road_segment"]:
|
||||||
|
# seg_info = map_api.get("road_segment", id)
|
||||||
|
# assert seg_info["token"] == id
|
||||||
|
# polygon = map_api.extract_polygon(seg_info["polygon_token"])
|
||||||
|
# polygons.append(polygon)
|
||||||
|
# for id in map_objs["road_block"]:
|
||||||
|
# seg_info = map_api.get("road_block", id)
|
||||||
|
# assert seg_info["token"] == id
|
||||||
|
# polygon = map_api.extract_polygon(seg_info["polygon_token"])
|
||||||
|
# polygons.append(polygon)
|
||||||
|
polygons = [geom if geom.is_valid else geom.buffer(0) for geom in polygons]
|
||||||
|
boundaries = gpd.GeoSeries(unary_union(polygons)).boundary.explode(index_parts=True)
|
||||||
|
for idx, boundary in enumerate(boundaries[0]):
|
||||||
|
block_points = np.array(list(i for i in zip(boundary.coords.xy[0], boundary.coords.xy[1])))
|
||||||
|
id = "boundary_{}".format(idx)
|
||||||
|
ret[id] = {
|
||||||
|
SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_WHITE,
|
||||||
|
SD.POLYLINE: block_points,
|
||||||
|
}
|
||||||
|
|
||||||
|
# broken line
|
||||||
|
# for id in map_objs["lane_divider"]:
|
||||||
|
# line_info = map_api.get("lane_divider", id)
|
||||||
|
# assert line_info["token"] == id
|
||||||
|
# line = map_api.extract_line(line_info["line_token"]).coords.xy
|
||||||
|
# line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))])
|
||||||
|
# ret[id] = {SD.TYPE: MetaDriveType.LINE_BROKEN_SINGLE_WHITE, SD.POLYLINE: line}
|
||||||
|
|
||||||
|
# # solid line
|
||||||
|
# for id in map_objs["road_divider"]:
|
||||||
|
# line_info = map_api.get("road_divider", id)
|
||||||
|
# assert line_info["token"] == id
|
||||||
|
# line = map_api.extract_line(line_info["line_token"]).coords.xy
|
||||||
|
# line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))])
|
||||||
|
# ret[id] = {SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_YELLOW, SD.POLYLINE: line}
|
||||||
|
|
||||||
|
# crosswalk
|
||||||
|
for id in map_objs["ped_crossing"]:
|
||||||
|
info = map_api.get("ped_crossing", id)
|
||||||
|
assert info["token"] == id
|
||||||
|
boundary = map_api.extract_polygon(info["polygon_token"]).exterior.xy
|
||||||
|
boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
||||||
|
ret[id] = {
|
||||||
|
SD.TYPE: MetaDriveType.CROSSWALK,
|
||||||
|
SD.POLYGON: boundary_polygon,
|
||||||
|
}
|
||||||
|
|
||||||
|
# walkway
|
||||||
|
for id in map_objs["walkway"]:
|
||||||
|
info = map_api.get("walkway", id)
|
||||||
|
assert info["token"] == id
|
||||||
|
boundary = map_api.extract_polygon(info["polygon_token"]).exterior.xy
|
||||||
|
boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
||||||
|
ret[id] = {
|
||||||
|
SD.TYPE: MetaDriveType.BOUNDARY_SIDEWALK,
|
||||||
|
SD.POLYGON: boundary_polygon,
|
||||||
|
}
|
||||||
|
|
||||||
|
# normal lane
|
||||||
|
for id in map_objs["lane"]:
|
||||||
|
lane_info = map_api.get("lane", id)
|
||||||
|
assert lane_info["token"] == id
|
||||||
|
boundary = map_api.extract_polygon(lane_info["polygon_token"]).boundary.xy
|
||||||
|
boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
||||||
|
# boundary_polygon += [[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))]
|
||||||
|
ret[id] = {
|
||||||
|
SD.TYPE: MetaDriveType.LANE_SURFACE_STREET,
|
||||||
|
SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)),
|
||||||
|
SD.POLYGON: boundary_polygon,
|
||||||
|
SD.ENTRY: map_api.get_incoming_lane_ids(id),
|
||||||
|
SD.EXIT: map_api.get_outgoing_lane_ids(id),
|
||||||
|
SD.LEFT_NEIGHBORS: [],
|
||||||
|
SD.RIGHT_NEIGHBORS: [],
|
||||||
|
}
|
||||||
|
|
||||||
|
# intersection lane
|
||||||
|
for id in map_objs["lane_connector"]:
|
||||||
|
lane_info = map_api.get("lane_connector", id)
|
||||||
|
assert lane_info["token"] == id
|
||||||
|
# boundary = map_api.extract_polygon(lane_info["polygon_token"]).boundary.xy
|
||||||
|
# boundary_polygon = [[boundary[0][i], boundary[1][i], 0.1] for i in range(len(boundary[0]))]
|
||||||
|
# boundary_polygon += [[boundary[0][i], boundary[1][i], 0.] for i in range(len(boundary[0]))]
|
||||||
|
ret[id] = {
|
||||||
|
SD.TYPE: MetaDriveType.LANE_SURFACE_UNSTRUCTURE,
|
||||||
|
SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)),
|
||||||
|
# SD.POLYGON: boundary_polygon,
|
||||||
|
"speed_limit_kmh": 100,
|
||||||
|
SD.ENTRY: map_api.get_incoming_lane_ids(id),
|
||||||
|
SD.EXIT: map_api.get_outgoing_lane_ids(id),
|
||||||
|
}
|
||||||
|
|
||||||
|
# # stop_line
|
||||||
|
# for id in map_objs["stop_line"]:
|
||||||
|
# info = map_api.get("stop_line", id)
|
||||||
|
# assert info["token"] == id
|
||||||
|
# boundary = map_api.extract_polygon(info["polygon_token"]).exterior.xy
|
||||||
|
# boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))])
|
||||||
|
# ret[id] = {
|
||||||
|
# SD.TYPE: MetaDriveType.STOP_LINE,
|
||||||
|
# SD.POLYGON: boundary_polygon ,
|
||||||
|
# }
|
||||||
|
|
||||||
|
# 'stop_line',
|
||||||
|
# 'carpark_area',
|
||||||
|
|
||||||
|
return ret
|
||||||
|
|
||||||
|
|
||||||
|
def convert_vod_scenario(
|
||||||
|
token,
|
||||||
|
version,
|
||||||
|
vodelft: VOD,
|
||||||
|
map_radius=500,
|
||||||
|
prediction=False,
|
||||||
|
past=2,
|
||||||
|
future=6,
|
||||||
|
only_lane=False,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Data will be interpolated to 0.1s time interval, while the time interval of original key frames are 0.5s.
|
||||||
|
"""
|
||||||
|
if prediction:
|
||||||
|
past_num = int(float(past) / 0.1)
|
||||||
|
future_num = int(float(future) / 0.1)
|
||||||
|
vode = vodelft
|
||||||
|
instance_token, sample_token = token.split("_")
|
||||||
|
current_sample = last_sample = next_sample = vode.get("sample", sample_token)
|
||||||
|
past_samples = []
|
||||||
|
future_samples = []
|
||||||
|
for _ in range(past_num):
|
||||||
|
if last_sample["prev"] == "":
|
||||||
|
break
|
||||||
|
last_sample = vode.get("sample", last_sample["prev"])
|
||||||
|
past_samples.append(parse_frame(last_sample, vode))
|
||||||
|
|
||||||
|
for _ in range(future_num):
|
||||||
|
if next_sample["next"] == "":
|
||||||
|
break
|
||||||
|
next_sample = vode.get("sample", next_sample["next"])
|
||||||
|
future_samples.append(parse_frame(next_sample, vode))
|
||||||
|
frames = (past_samples[::-1] + [parse_frame(current_sample, vode)] + future_samples)
|
||||||
|
scene_info = copy.copy(vode.get("scene", current_sample["scene_token"]))
|
||||||
|
scene_info["name"] = scene_info["name"] + "_" + token
|
||||||
|
scene_info["prediction"] = True
|
||||||
|
frames_scene_info = [frames, scene_info]
|
||||||
|
else:
|
||||||
|
frames_scene_info = extract_frames_scene_info(token, vodelft)
|
||||||
|
|
||||||
|
scenario_log_interval = 0.1
|
||||||
|
frames, scene_info = frames_scene_info
|
||||||
|
result = SD()
|
||||||
|
result[SD.ID] = scene_info["name"]
|
||||||
|
result[SD.VERSION] = "vod" + version
|
||||||
|
result[SD.LENGTH] = len(frames)
|
||||||
|
result[SD.METADATA] = {}
|
||||||
|
result[SD.METADATA]["dataset"] = "vod"
|
||||||
|
result[SD.METADATA][SD.METADRIVE_PROCESSED] = False
|
||||||
|
result[SD.METADATA]["map"] = vodelft.get("log", scene_info["log_token"])["location"]
|
||||||
|
result[SD.METADATA]["date"] = vodelft.get("log", scene_info["log_token"])["date_captured"]
|
||||||
|
result[SD.METADATA]["coordinate"] = "right-handed"
|
||||||
|
# result[SD.METADATA]["dscenario_token"] = scene_token
|
||||||
|
result[SD.METADATA][SD.ID] = scene_info["name"]
|
||||||
|
result[SD.METADATA]["scenario_id"] = scene_info["name"]
|
||||||
|
result[SD.METADATA]["sample_rate"] = scenario_log_interval
|
||||||
|
result[SD.METADATA][SD.TIMESTEP] = np.arange(0.0, len(frames), 1) * 0.1
|
||||||
|
# interpolating to 0.1s interval
|
||||||
|
result[SD.TRACKS] = get_tracks_from_frames(vodelft, scene_info, frames, num_to_interpolate=1)
|
||||||
|
result[SD.METADATA][SD.SDC_ID] = "ego"
|
||||||
|
|
||||||
|
# No traffic light in vod at this stage
|
||||||
|
result[SD.DYNAMIC_MAP_STATES] = {}
|
||||||
|
if prediction:
|
||||||
|
track_to_predict = result[SD.TRACKS][instance_token]
|
||||||
|
result[SD.METADATA]["tracks_to_predict"] = {
|
||||||
|
instance_token: {
|
||||||
|
"track_index": list(result[SD.TRACKS].keys()).index(instance_token),
|
||||||
|
"track_id": instance_token,
|
||||||
|
"difficulty": 0,
|
||||||
|
"object_type": track_to_predict["type"],
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
# map
|
||||||
|
print(result[SD.LENGTH], len(result[SD.METADATA][SD.TIMESTEP]))
|
||||||
|
map_center = np.array(result[SD.TRACKS]["ego"]["state"]["position"][0])
|
||||||
|
result[SD.MAP_FEATURES] = get_map_features(scene_info, vodelft, map_center, map_radius, only_lane=only_lane)
|
||||||
|
del frames_scene_info
|
||||||
|
del frames
|
||||||
|
del scene_info
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def extract_frames_scene_info(scene, vod):
|
||||||
|
scene_token = scene["token"]
|
||||||
|
scene_info = vod.get("scene", scene_token)
|
||||||
|
scene_info["nbr_samples"] -= 1
|
||||||
|
frames = []
|
||||||
|
current_frame = vod.get("sample", scene_info["first_sample_token"])
|
||||||
|
while current_frame["token"] != scene_info["last_sample_token"]:
|
||||||
|
frames.append(parse_frame(current_frame, vod))
|
||||||
|
current_frame = vod.get("sample", current_frame["next"])
|
||||||
|
frames.append(parse_frame(current_frame, vod))
|
||||||
|
frames = frames[1:]
|
||||||
|
assert current_frame["next"] == ""
|
||||||
|
assert len(frames) == scene_info["nbr_samples"], "Number of sample mismatches! "
|
||||||
|
return frames, scene_info
|
||||||
|
|
||||||
|
|
||||||
|
def get_vod_scenarios(dataroot, version, num_workers=2):
|
||||||
|
vode = VOD(version=version, dataroot=dataroot)
|
||||||
|
|
||||||
|
return vode.scene, [vode for _ in range(num_workers)]
|
||||||
|
|
||||||
|
|
||||||
|
def get_vod_prediction_split(dataroot, version, past, future, num_workers=2):
|
||||||
|
# TODO do properly
|
||||||
|
split_to_scene = {
|
||||||
|
"mini_train": "v1.0-mini",
|
||||||
|
"mini_val": "v1.0-mini",
|
||||||
|
"train": "v1.0-trainval",
|
||||||
|
"train_val": "v1.0-trainval",
|
||||||
|
"val": "v1.0-trainval",
|
||||||
|
"test": "v1.0-test",
|
||||||
|
}
|
||||||
|
|
||||||
|
vode = VOD(version=split_to_scene[version], dataroot=dataroot)
|
||||||
|
|
||||||
|
return get_prediction_challenge_split(version, dataroot=dataroot), [vode for _ in range(num_workers)]
|
||||||
@@ -430,6 +430,11 @@ def get_waymo_scenarios(waymo_data_directory, start_index, num):
|
|||||||
# there is 1000 raw data in google cloud, each of them produce about 500 pkl file
|
# there is 1000 raw data in google cloud, each of them produce about 500 pkl file
|
||||||
logger.info("\nReading raw data")
|
logger.info("\nReading raw data")
|
||||||
file_list = os.listdir(waymo_data_directory)
|
file_list = os.listdir(waymo_data_directory)
|
||||||
|
if num is None:
|
||||||
|
logger.warning(
|
||||||
|
"You haven't specified the number of raw files! It is set to {} now.".format(len(file_list) - start_index)
|
||||||
|
)
|
||||||
|
num = len(file_list) - start_index
|
||||||
assert len(file_list) >= start_index + num and start_index >= 0, \
|
assert len(file_list) >= start_index + num and start_index >= 0, \
|
||||||
"No sufficient files ({}) in raw_data_directory. need: {}, start: {}".format(len(file_list), num, start_index)
|
"No sufficient files ({}) in raw_data_directory. need: {}, start: {}".format(len(file_list), num, start_index)
|
||||||
file_list = file_list[start_index:start_index + num]
|
file_list = file_list[start_index:start_index + num]
|
||||||
@@ -448,9 +453,13 @@ def preprocess_waymo_scenarios(files, worker_index):
|
|||||||
"""
|
"""
|
||||||
from scenarionet.converter.waymo.waymo_protos import scenario_pb2
|
from scenarionet.converter.waymo.waymo_protos import scenario_pb2
|
||||||
|
|
||||||
for file in tqdm.tqdm(files, desc="Process Waymo scenarios for worker {}".format(worker_index)):
|
for file in tqdm.tqdm(files, leave=False, position=0, desc="Worker {} Number of raw file".format(worker_index)):
|
||||||
|
|
||||||
|
logger.info(f"Worker {worker_index} is reading raw file: {file}")
|
||||||
|
|
||||||
file_path = os.path.join(file)
|
file_path = os.path.join(file)
|
||||||
if ("tfrecord" not in file_path) or (not os.path.isfile(file_path)):
|
if ("tfrecord" not in file_path) or (not os.path.isfile(file_path)):
|
||||||
|
logger.info(f"Worker {worker_index} skip this file: {file}")
|
||||||
continue
|
continue
|
||||||
for data in tf.data.TFRecordDataset(file_path, compression_type="").as_numpy_iterator():
|
for data in tf.data.TFRecordDataset(file_path, compression_type="").as_numpy_iterator():
|
||||||
scenario = scenario_pb2.Scenario()
|
scenario = scenario_pb2.Scenario()
|
||||||
@@ -458,5 +467,7 @@ def preprocess_waymo_scenarios(files, worker_index):
|
|||||||
# a trick for loging file name
|
# a trick for loging file name
|
||||||
scenario.scenario_id = scenario.scenario_id + SPLIT_KEY + file
|
scenario.scenario_id = scenario.scenario_id + SPLIT_KEY + file
|
||||||
yield scenario
|
yield scenario
|
||||||
|
|
||||||
|
logger.info(f"Worker {worker_index} finished read {len(files)} files.")
|
||||||
# logger.info("Worker {}: Process {} waymo scenarios".format(worker_index, len(scenarios)))
|
# logger.info("Worker {}: Process {} waymo scenarios".format(worker_index, len(scenarios)))
|
||||||
# return scenarios
|
# return scenarios
|
||||||
|
|||||||
@@ -3,6 +3,7 @@ desc = "Filter unwanted scenarios out and build a new database"
|
|||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
import argparse
|
import argparse
|
||||||
|
|
||||||
|
from metadrive.type import MetaDriveType
|
||||||
from scenarionet.builder.filters import ScenarioFilter
|
from scenarionet.builder.filters import ScenarioFilter
|
||||||
from scenarionet.builder.utils import merge_database
|
from scenarionet.builder.utils import merge_database
|
||||||
|
|
||||||
@@ -59,6 +60,31 @@ if __name__ == '__main__':
|
|||||||
"--exclude_ids", nargs='+', default=[], help="Scenarios with indicated name will NOT be selected"
|
"--exclude_ids", nargs='+', default=[], help="Scenarios with indicated name will NOT be selected"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
parser.add_argument(
|
||||||
|
"--num_vehicle",
|
||||||
|
action="store_true",
|
||||||
|
help="add this flag to select cases with vehicle_num < max_num_vehicle"
|
||||||
|
)
|
||||||
|
|
||||||
|
parser.add_argument(
|
||||||
|
"--max_num_vehicle",
|
||||||
|
default=50,
|
||||||
|
type=int,
|
||||||
|
help="case will be selected if num_vehicle < this argument"
|
||||||
|
)
|
||||||
|
|
||||||
|
parser.add_argument(
|
||||||
|
"--no_pedestrian",
|
||||||
|
action="store_true",
|
||||||
|
help="Scenarios with pedestrians WON'T be selected"
|
||||||
|
)
|
||||||
|
|
||||||
|
parser.add_argument(
|
||||||
|
"--no_cyclist",
|
||||||
|
action="store_true",
|
||||||
|
help="Scenarios with cyclists WON'T be selected"
|
||||||
|
)
|
||||||
|
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
target = args.sdc_moving_dist_min
|
target = args.sdc_moving_dist_min
|
||||||
obj_threshold = args.max_num_object
|
obj_threshold = args.max_num_object
|
||||||
@@ -75,6 +101,19 @@ if __name__ == '__main__':
|
|||||||
filters.append(ScenarioFilter.make(ScenarioFilter.no_traffic_light))
|
filters.append(ScenarioFilter.make(ScenarioFilter.no_traffic_light))
|
||||||
if args.id_filter:
|
if args.id_filter:
|
||||||
filters.append(ScenarioFilter.make(ScenarioFilter.id_filter, ids=args.exclude_ids))
|
filters.append(ScenarioFilter.make(ScenarioFilter.id_filter, ids=args.exclude_ids))
|
||||||
|
if args.num_vehicle:
|
||||||
|
filters.append(
|
||||||
|
ScenarioFilter.make(
|
||||||
|
ScenarioFilter.object_number,
|
||||||
|
number_threshold=args.max_num_vehicle,
|
||||||
|
object_type=MetaDriveType.VEHICLE,
|
||||||
|
condition=ScenarioFilter.SMALLER
|
||||||
|
)
|
||||||
|
)
|
||||||
|
if args.no_pedestrian:
|
||||||
|
filters.append(ScenarioFilter.make(ScenarioFilter.no_pedestrian))
|
||||||
|
if args.no_cyclist:
|
||||||
|
filters.append(ScenarioFilter.make(ScenarioFilter.no_cyclist))
|
||||||
|
|
||||||
if len(filters) == 0:
|
if len(filters) == 0:
|
||||||
raise ValueError("No filters are applied. Abort.")
|
raise ValueError("No filters are applied. Abort.")
|
||||||
|
|||||||
@@ -29,7 +29,7 @@ if __name__ == "__main__":
|
|||||||
"vehicle_config": dict(
|
"vehicle_config": dict(
|
||||||
show_navi_mark=False,
|
show_navi_mark=False,
|
||||||
use_special_color=False,
|
use_special_color=False,
|
||||||
image_source="semantic_camera",
|
# image_source="semantic_camera",
|
||||||
lidar=dict(num_lasers=120, distance=50),
|
lidar=dict(num_lasers=120, distance=50),
|
||||||
lane_line_detector=dict(num_lasers=0, distance=50),
|
lane_line_detector=dict(num_lasers=0, distance=50),
|
||||||
side_detector=dict(num_lasers=12, distance=50)
|
side_detector=dict(num_lasers=12, distance=50)
|
||||||
@@ -43,11 +43,11 @@ if __name__ == "__main__":
|
|||||||
"camera_height": 1.5,
|
"camera_height": 1.5,
|
||||||
"camera_pitch": None,
|
"camera_pitch": None,
|
||||||
"camera_fov": 60,
|
"camera_fov": 60,
|
||||||
"interface_panel": ["semantic_camera"],
|
# "interface_panel": ["semantic_camera"],
|
||||||
"sensors": dict(
|
"sensors": dict(
|
||||||
semantic_camera=(SemanticCamera, 1600, 900),
|
# semantic_camera=(SemanticCamera, 1600, 900),
|
||||||
depth_camera=(DepthCamera, 800, 600),
|
# depth_camera=(DepthCamera, 800, 600),
|
||||||
rgb_camera=(RGBCamera, 800, 600),
|
rgb_camera=(RGBCamera, 1600, 900),
|
||||||
),
|
),
|
||||||
|
|
||||||
# ===== Remove useless items in the images =====
|
# ===== Remove useless items in the images =====
|
||||||
@@ -66,8 +66,8 @@ if __name__ == "__main__":
|
|||||||
# Run it once to initialize the TopDownRenderer
|
# Run it once to initialize the TopDownRenderer
|
||||||
env.render(
|
env.render(
|
||||||
mode="topdown",
|
mode="topdown",
|
||||||
screen_size=(1600, 900),
|
screen_size=(900, 900), # The output image size
|
||||||
film_size=(9000, 9000),
|
film_size=(9000, 9000), # The internal canvas size. You can use this to "crop" images.
|
||||||
target_vehicle_heading_up=True,
|
target_vehicle_heading_up=True,
|
||||||
semantic_map=True,
|
semantic_map=True,
|
||||||
)
|
)
|
||||||
@@ -86,10 +86,16 @@ if __name__ == "__main__":
|
|||||||
to_image=False
|
to_image=False
|
||||||
)
|
)
|
||||||
pygame.image.save(ret, str(file_dir / "bev_{}.png".format(t)))
|
pygame.image.save(ret, str(file_dir / "bev_{}.png".format(t)))
|
||||||
env.engine.get_sensor("depth_camera").save_image(env.agent, str(file_dir / "depth_{}.jpg".format(t)))
|
# env.engine.get_sensor("depth_camera").save_image(env.agent, str(file_dir / "depth_{}.jpg".format(t)))
|
||||||
env.engine.get_sensor("rgb_camera").save_image(env.agent, str(file_dir / "rgb_{}.jpg".format(t)))
|
env.engine.get_sensor("rgb_camera").save_image(env.agent, str(file_dir / "rgb_{}.jpg".format(t)))
|
||||||
env.engine.get_sensor("semantic_camera").save_image(env.agent, str(file_dir / "semantic_{}.jpg".format(t)))
|
# env.engine.get_sensor("semantic_camera").save_image(env.agent, str(file_dir / "semantic_{}.jpg".format(t)))
|
||||||
print("Image at step {} is saved at: {}".format(t, file_dir))
|
print("Image at step {} is saved at: {}".format(t, file_dir))
|
||||||
|
|
||||||
|
scenario = env.engine.data_manager.current_scenario
|
||||||
|
print(
|
||||||
|
f"Current scenario ID {scenario['id']}, dataset version {scenario['version']}, len: {scenario['length']}"
|
||||||
|
)
|
||||||
|
|
||||||
if t == 30:
|
if t == 30:
|
||||||
break
|
break
|
||||||
env.step([1, 0.88])
|
env.step([1, 0.88])
|
||||||
|
|||||||
@@ -6,7 +6,7 @@ from typing import List
|
|||||||
|
|
||||||
from metadrive.scenario.scenario_description import ScenarioDescription as SD
|
from metadrive.scenario.scenario_description import ScenarioDescription as SD
|
||||||
|
|
||||||
from scenarionet.common_utils import save_summary_anda_mapping, read_dataset_summary
|
from scenarionet.common_utils import save_summary_and_mapping, read_dataset_summary
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
@@ -98,5 +98,5 @@ class ErrorFile:
|
|||||||
new_summary[scenario] = origin_summary[scenario]
|
new_summary[scenario] = origin_summary[scenario]
|
||||||
scenario_dir = os.path.join(origin_dataset_path, origin_mapping[scenario])
|
scenario_dir = os.path.join(origin_dataset_path, origin_mapping[scenario])
|
||||||
new_mapping[scenario] = os.path.relpath(scenario_dir, new_dataset_path)
|
new_mapping[scenario] = os.path.relpath(scenario_dir, new_dataset_path)
|
||||||
save_summary_anda_mapping(new_summary_file_path, new_mapping_file_path, new_summary, new_mapping)
|
save_summary_and_mapping(new_summary_file_path, new_mapping_file_path, new_summary, new_mapping)
|
||||||
return new_summary, new_mapping
|
return new_summary, new_mapping
|
||||||
|
|||||||
Reference in New Issue
Block a user