├── navsim ├── __init__.py ├── agents │ ├── __init__.py │ ├── SeerDrive │ │ ├── __init__.py │ │ ├── modules │ │ │ ├── blocks.py │ │ │ └── multimodal_loss.py │ │ ├── SeerDrive_features.py │ │ └── configs │ │ │ └── default.py │ ├── human_agent.py │ ├── constant_velocity_agent.py │ ├── transfuser │ │ ├── transfuser_agent.py │ │ ├── transfuser_config.py │ │ ├── transfuser_loss.py │ │ └── transfuser_model.py │ ├── ego_status_mlp_agent.py │ └── abstract_agent.py ├── common │ ├── __init__.py │ ├── enums.py │ └── dataloader.py ├── evaluate │ └── __init__.py ├── planning │ ├── __init__.py │ ├── script │ │ ├── __init__.py │ │ ├── builders │ │ │ ├── __init__.py │ │ │ ├── observation_builder.py │ │ │ ├── worker_pool_builder.py │ │ │ ├── planner_builder.py │ │ │ └── simulation_builder.py │ │ ├── config │ │ │ ├── __init__.py │ │ │ ├── common │ │ │ │ ├── __init__.py │ │ │ │ ├── worker │ │ │ │ │ ├── __init__.py │ │ │ │ │ ├── sequential.yaml │ │ │ │ │ ├── single_machine_thread_pool.yaml │ │ │ │ │ ├── ray_distributed.yaml │ │ │ │ │ └── ray_distributed_no_torch.yaml │ │ │ │ ├── scene_filter │ │ │ │ │ ├── __init__.py │ │ │ │ │ └── all_scenes.yaml │ │ │ │ ├── agent │ │ │ │ │ ├── human_agent.yaml │ │ │ │ │ ├── constant_velocity_agent.yaml │ │ │ │ │ ├── ego_status_mlp_agent.yaml │ │ │ │ │ ├── SeerDrive_agent.yaml │ │ │ │ │ └── transfuser_agent.yaml │ │ │ │ ├── default_evaluation.yaml │ │ │ │ └── default_common.yaml │ │ │ ├── pdm_scoring │ │ │ │ ├── __init__.py │ │ │ │ ├── default_run_pdm_score.yaml │ │ │ │ ├── default_run_pdm_score_from_submission.yaml │ │ │ │ ├── default_run_create_submission_pickle.yaml │ │ │ │ └── default_scoring_parameters.yaml │ │ │ ├── training │ │ │ │ ├── __init__.py │ │ │ │ └── default_training.yaml │ │ │ └── metric_caching │ │ │ │ ├── __init__.py │ │ │ │ └── default_metric_caching.yaml │ │ ├── run_metric_caching.py │ │ ├── run_dataset_caching.py │ │ ├── run_create_submission_pickle.py │ │ ├── run_pdm_score_from_submission.py │ │ ├── run_training.py │ │ └── utils.py │ ├── simulation │ │ ├── __init__.py │ │ └── planner │ │ │ ├── __init__.py │ │ │ └── pdm_planner │ │ │ ├── __init__.py │ │ │ ├── proposal │ │ │ ├── __init__.py │ │ │ └── pdm_proposal.py │ │ │ ├── scoring │ │ │ ├── __init__.py │ │ │ └── pdm_scorer_utils.py │ │ │ ├── utils │ │ │ ├── __init__.py │ │ │ ├── graph_search │ │ │ │ ├── __init__.py │ │ │ │ ├── bfs_roadblock.py │ │ │ │ └── dijkstra.py │ │ │ ├── pdm_geometry_utils.py │ │ │ ├── pdm_enums.py │ │ │ ├── pdm_path.py │ │ │ └── pdm_emergency_brake.py │ │ │ ├── observation │ │ │ └── __init__.py │ │ │ ├── simulation │ │ │ ├── __init__.py │ │ │ └── pdm_simulator.py │ │ │ ├── pdm_closed_planner.py │ │ │ └── abstract_pdm_closed_planner.py │ ├── training │ │ ├── __init__.py │ │ ├── abstract_feature_target_builder.py │ │ ├── callbacks │ │ │ └── time_logging_callback.py │ │ └── agent_lightning_module.py │ ├── metric_caching │ │ ├── __init__.py │ │ ├── metric_cache.py │ │ └── metric_caching_utils.py │ ├── scenario_builder │ │ ├── __init__.py │ │ └── navsim_scenario_utils.py │ └── utils │ │ └── multithreading │ │ └── __init__.py └── visualization │ ├── __init__.py │ └── lidar.py ├── asset ├── main.png └── visual.png ├── scripts ├── miscs │ ├── gen_pdm_score.sh │ └── k_means_trajs.py ├── evaluation │ └── run_metric_caching.sh └── training │ ├── seerdrive_train.sh │ └── seerdrive_eval.sh ├── environment.yml ├── .gitignore ├── docs ├── cache.md ├── install.md ├── metrics.md ├── submission.md └── splits.md ├── setup.py ├── LICENSE ├── requirements.txt └── README.md /navsim/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/agents/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/common/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/evaluate/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/visualization/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/agents/SeerDrive/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/training/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/metric_caching/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/builders/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/config/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/scenario_builder/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/utils/multithreading/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/config/pdm_scoring/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/config/training/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/worker/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/config/metric_caching/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/scene_filter/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/proposal/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/scoring/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/observation/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/simulation/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/graph_search/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /asset/main.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LogosRoboticsGroup/SeerDrive/HEAD/asset/main.png -------------------------------------------------------------------------------- /asset/visual.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LogosRoboticsGroup/SeerDrive/HEAD/asset/visual.png -------------------------------------------------------------------------------- /navsim/planning/script/config/common/agent/human_agent.yaml: -------------------------------------------------------------------------------- 1 | _target_: navsim.agents.human_agent.HumanAgent 2 | _convert_: 'all' -------------------------------------------------------------------------------- /navsim/planning/script/config/common/agent/constant_velocity_agent.yaml: -------------------------------------------------------------------------------- 1 | _target_: navsim.agents.constant_velocity_agent.ConstantVelocityAgent 2 | _convert_: 'all' -------------------------------------------------------------------------------- /navsim/planning/script/config/common/worker/sequential.yaml: -------------------------------------------------------------------------------- 1 | _target_: nuplan.planning.utils.multithreading.worker_sequential.Sequential 2 | _convert_: 'all' 3 | -------------------------------------------------------------------------------- /scripts/miscs/gen_pdm_score.sh: -------------------------------------------------------------------------------- 1 | python scripts/misc/gen_multi_trajs_pdm_score.py \ 2 | metric_cache_path='/Path_To_SeerDrive/exp/metric_cache/trainval' \ 3 | split=trainval \ 4 | scene_filter=navtrain 5 | -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: navsim_seerdrive 2 | channels: 3 | - conda-forge 4 | dependencies: 5 | - python=3.9 6 | - pip=23.3.1 7 | - nb_conda_kernels 8 | - pip: 9 | - -r requirements.txt 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # python 2 | *.pyc 3 | **/__pycache__/ 4 | .pytest_cache/* 5 | .pydevproject 6 | 7 | # IDE 8 | .vscode/* 9 | 10 | # Pip 11 | *.egg-info 12 | 13 | # files 14 | *.log 15 | *.pkl 16 | *.pcd 17 | 18 | nuplan/ 19 | exp/ 20 | download/ 21 | dataset/ 22 | ckpts 23 | 24 | vis/ 25 | extra_data/ 26 | 27 | trans.sh 28 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/agent/ego_status_mlp_agent.yaml: -------------------------------------------------------------------------------- 1 | _target_: navsim.agents.ego_status_mlp_agent.EgoStatusMLPAgent 2 | _convert_: 'all' 3 | 4 | trajectory_sampling: 5 | _target_: nuplan.planning.simulation.trajectory.trajectory_sampling.TrajectorySampling 6 | _convert_: 'all' 7 | time_horizon: 4 8 | interval_length: 0.5 9 | checkpoint_path: null 10 | hidden_layer_dim: 512 11 | lr: 1e-4 12 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/worker/single_machine_thread_pool.yaml: -------------------------------------------------------------------------------- 1 | _target_: nuplan.planning.utils.multithreading.worker_parallel.SingleMachineParallelExecutor 2 | _convert_: 'all' 3 | use_process_pool: False # If true, use ProcessPoolExecutor as the backend, otherwise uses ThreadPoolExecutor 4 | max_workers: null # Number of CPU workers (threads/processes) to use per node, "null" means all available 5 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/default_evaluation.yaml: -------------------------------------------------------------------------------- 1 | # Cache parameters 2 | experiment_name: ??? 3 | navsim_log_path: ${oc.env:OPENSCENE_DATA_ROOT}/navsim_logs/${split} # path to log annotations 4 | sensor_blobs_path: ${oc.env:OPENSCENE_DATA_ROOT}/sensor_blobs/${split} # path to sensor blobs 5 | date_format: '%Y.%m.%d.%H.%M.%S' 6 | experiment_uid: ${now:${date_format}} 7 | output_dir: ${oc.env:NAVSIM_EXP_ROOT}/${experiment_name}/${experiment_uid} # path where output csv is saved -------------------------------------------------------------------------------- /navsim/planning/script/config/common/agent/SeerDrive_agent.yaml: -------------------------------------------------------------------------------- 1 | _target_: navsim.agents.SeerDrive.SeerDrive_agent.SeerDriveAgent 2 | _convert_: 'all' 3 | 4 | config: 5 | _target_: navsim.agents.SeerDrive.configs.default.SeerDriveConfig 6 | _convert_: 'all' 7 | 8 | trajectory_sampling: 9 | _target_: nuplan.planning.simulation.trajectory.trajectory_sampling.TrajectorySampling 10 | _convert_: 'all' 11 | time_horizon: 4 12 | interval_length: 0.5 13 | 14 | checkpoint_path: null 15 | lr: 2e-4 16 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/agent/transfuser_agent.yaml: -------------------------------------------------------------------------------- 1 | _target_: navsim.agents.transfuser.transfuser_agent.TransfuserAgent 2 | _convert_: 'all' 3 | 4 | config: 5 | _target_: navsim.agents.transfuser.transfuser_config.TransfuserConfig 6 | _convert_: 'all' 7 | 8 | trajectory_sampling: 9 | _target_: nuplan.planning.simulation.trajectory.trajectory_sampling.TrajectorySampling 10 | _convert_: 'all' 11 | time_horizon: 4 12 | interval_length: 0.5 13 | 14 | checkpoint_path: null 15 | lr: 1e-4 16 | -------------------------------------------------------------------------------- /navsim/planning/script/config/pdm_scoring/default_run_pdm_score.yaml: -------------------------------------------------------------------------------- 1 | hydra: 2 | run: 3 | dir: ${output_dir} 4 | output_subdir: ${output_dir}/code/hydra # Store hydra's config breakdown here for debugging 5 | searchpath: # Only in these paths are discoverable 6 | - pkg://navsim.planning.script.config.common 7 | 8 | defaults: 9 | - default_common 10 | - default_evaluation 11 | - default_scoring_parameters 12 | - agent: constant_velocity_agent 13 | 14 | metric_cache_path: ${oc.env:NAVSIM_EXP_ROOT}/metric_cache # path to metric cache -------------------------------------------------------------------------------- /navsim/planning/script/config/pdm_scoring/default_run_pdm_score_from_submission.yaml: -------------------------------------------------------------------------------- 1 | hydra: 2 | run: 3 | dir: ${output_dir} 4 | output_subdir: ${output_dir}/code/hydra # Store hydra's config breakdown here for debugging 5 | searchpath: # Only in these paths are discoverable 6 | - pkg://navsim.planning.script.config.common 7 | 8 | defaults: 9 | - default_common 10 | - default_scoring_parameters 11 | 12 | metric_cache_path: ${oc.env:NAVSIM_EXP_ROOT}/metric_cache # path to metric cache 13 | submission_file_path: ??? # path to submission file 14 | output_dir: ??? -------------------------------------------------------------------------------- /scripts/evaluation/run_metric_caching.sh: -------------------------------------------------------------------------------- 1 | SPLIT=test # SPLIT=trainval 2 | 3 | export PYTHONPATH="/Path_To_SeerDrive" 4 | export NUPLAN_MAP_VERSION="nuplan-maps-v1.0" 5 | export NUPLAN_MAPS_ROOT="/Path_To_Dataset/openscene/maps" 6 | export NAVSIM_EXP_ROOT="/Path_To_SeerDrive/exp" 7 | export NAVSIM_DEVKIT_ROOT="/Path_To_SeerDrive" 8 | export OPENSCENE_DATA_ROOT="/Path_To_Dataset/openscene" 9 | export CUDA_VISIBLE_DEVICES=0,1,2,3,4,5,6,7 10 | 11 | python $NAVSIM_DEVKIT_ROOT/navsim/planning/script/run_metric_caching.py \ 12 | split=$SPLIT \ 13 | cache.cache_path='/Path_To_SeerDrive/exp/metric_cache' \ 14 | scene_filter.frame_interval=1 15 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/worker/ray_distributed.yaml: -------------------------------------------------------------------------------- 1 | _target_: nuplan.planning.utils.multithreading.worker_ray.RayDistributed 2 | _convert_: 'all' 3 | master_node_ip: null # Set to a master node IP if you desire to connect to cluster remotely 4 | threads_per_node: null # Number of CPU threads to use per node, "null" means all threads available 5 | debug_mode: false # If true all tasks will be executed serially, mainly for testing 6 | log_to_driver: true # If true, all printouts from ray threads will be displayed in driver 7 | logs_subdir: 'logs' # Subdirectory to store logs inside the experiment directory 8 | use_distributed: false # Whether to use the built-in distributed mode of ray 9 | -------------------------------------------------------------------------------- /navsim/planning/script/config/common/worker/ray_distributed_no_torch.yaml: -------------------------------------------------------------------------------- 1 | _target_: navsim.planning.utils.multithreading.worker_ray_no_torch.RayDistributedNoTorch 2 | _convert_: 'all' 3 | master_node_ip: null # Set to a master node IP if you desire to connect to cluster remotely 4 | threads_per_node: null # Number of CPU threads to use per node, "null" means all threads available 5 | debug_mode: false # If true all tasks will be executed serially, mainly for testing 6 | log_to_driver: true # If true, all printouts from ray threads will be displayed in driver 7 | logs_subdir: 'logs' # Subdirectory to store logs inside the experiment directory 8 | use_distributed: false # Whether to use the built-in distributed mode of ray 9 | -------------------------------------------------------------------------------- /navsim/planning/script/config/metric_caching/default_metric_caching.yaml: -------------------------------------------------------------------------------- 1 | hydra: 2 | run: 3 | dir: ${output_dir} 4 | output_subdir: ${output_dir}/code/hydra # Store hydra's config breakdown here for debugging 5 | searchpath: # Only in these paths are discoverable 6 | - pkg://navsim.planning.script.config.common 7 | 8 | defaults: 9 | - default_common 10 | 11 | # Cache parameters 12 | cache: 13 | cache_path: ${oc.env:NAVSIM_EXP_ROOT}/metric_cache 14 | use_cache_without_dataset: false 15 | force_feature_computation: false 16 | 17 | output_dir: ${cache.cache_path}/metadata 18 | navsim_log_path: ${oc.env:OPENSCENE_DATA_ROOT}/navsim_logs/${split} # path to log annotations -------------------------------------------------------------------------------- /scripts/training/seerdrive_train.sh: -------------------------------------------------------------------------------- 1 | # Define 2 | export PYTHONPATH="/Path_To_SeerDrive" 3 | export NUPLAN_MAP_VERSION="nuplan-maps-v1.0" 4 | export NUPLAN_MAPS_ROOT="/Path_To_OpenScene/maps" 5 | export NAVSIM_EXP_ROOT="/Path_To_SeerDrive/exp" 6 | export NAVSIM_DEVKIT_ROOT="/Path_To_SeerDrive" 7 | export OPENSCENE_DATA_ROOT="/Path_To_OpenScene" 8 | export CUDA_VISIBLE_DEVICES=0,1,2,3,4,5,6,7 9 | 10 | CONFIG_NAME=default 11 | 12 | ### training ### 13 | python ./navsim/planning/script/run_training.py \ 14 | agent=SeerDrive_agent \ 15 | agent.config._target_=navsim.agents.SeerDrive.configs.${CONFIG_NAME}.SeerDriveConfig \ 16 | experiment_name=train \ 17 | scene_filter=navtrain \ 18 | dataloader.params.batch_size=16 \ 19 | trainer.params.max_epochs=32 \ 20 | split=trainval 21 | -------------------------------------------------------------------------------- /navsim/planning/script/builders/observation_builder.py: -------------------------------------------------------------------------------- 1 | from typing import cast 2 | 3 | from hydra.utils import instantiate 4 | from omegaconf import DictConfig 5 | 6 | from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario 7 | from nuplan.planning.simulation.observation.abstract_observation import AbstractObservation 8 | 9 | 10 | def build_observations(observation_cfg: DictConfig, scenario: AbstractScenario) -> AbstractObservation: 11 | """ 12 | Instantiate observations 13 | :param observation_cfg: config of a planner 14 | :param scenario: scenario 15 | :return AbstractObservation 16 | """ 17 | 18 | observation = cast(AbstractObservation, instantiate(observation_cfg, scenario=scenario)) 19 | 20 | return observation 21 | -------------------------------------------------------------------------------- /scripts/training/seerdrive_eval.sh: -------------------------------------------------------------------------------- 1 | # Define 2 | export PYTHONPATH="/Path_To_SeerDrive" 3 | export NUPLAN_MAP_VERSION="nuplan-maps-v1.0" 4 | export NUPLAN_MAPS_ROOT="/Path_To_OpenScene/maps" 5 | export NAVSIM_EXP_ROOT="/Path_To_SeerDrive/exp" 6 | export NAVSIM_DEVKIT_ROOT="/Path_To_SeerDrive" 7 | export OPENSCENE_DATA_ROOT="/Path_To_OpenScene" 8 | export CUDA_VISIBLE_DEVICES=0,1,2,3,4,5,6,7 9 | 10 | CONFIG_NAME=default 11 | 12 | ### evaluation ### 13 | export CKPT="YOUR_ABSOLUTE_PATH_TO_CHECKPOINT" 14 | 15 | python ./navsim/planning/script/run_pdm_score.py \ 16 | agent=SeerDrive_agent \ 17 | agent.checkpoint_path=$CKPT \ 18 | agent.config._target_=navsim.agents.SeerDrive.configs.${CONFIG_NAME}.SeerDriveConfig \ 19 | experiment_name=eval \ 20 | split=test \ 21 | scene_filter=navtest 22 | -------------------------------------------------------------------------------- /navsim/planning/script/config/pdm_scoring/default_run_create_submission_pickle.yaml: -------------------------------------------------------------------------------- 1 | hydra: 2 | run: 3 | dir: ${output_dir} 4 | output_subdir: ${output_dir}/code/hydra # Store hydra's config breakdown here for debugging 5 | searchpath: # Only in these paths are discoverable 6 | - pkg://navsim.planning.script.config.common 7 | 8 | defaults: 9 | - default_common 10 | - default_evaluation 11 | - agent: constant_velocity_agent 12 | - override scene_filter: private_test_e2e 13 | 14 | split: private_test_e2e 15 | 16 | "team_name": ??? # The team name 17 | "authors": ??? # The team members 18 | "email": ??? # email of the corresponding team member 19 | "institution": ??? # affiliation of the team 20 | "country": ??? # country or region of the team, e.g. China -------------------------------------------------------------------------------- /navsim/planning/script/config/common/scene_filter/all_scenes.yaml: -------------------------------------------------------------------------------- 1 | _target_: navsim.common.dataclasses.SceneFilter 2 | _convert_: 'all' 3 | 4 | num_history_frames: 4 # number of past frames to be extracted, frames are at 2Hz (1=ony current frame, 2=1 second) 5 | num_future_frames: 10 # number of future frames to be extracted, frames are at 2Hz (10=5 seconds) 6 | frame_interval: null # number of frames to skip between each scene, if null, extracted scenes are non-overlapping 7 | 8 | has_route: true # only extract scenes with valid route information 9 | 10 | max_scenes: null # maximum number of scenes to extract, if null, all scenes are extracted. If integer, scene loading stops when reaching it 11 | log_names: null # list of log names to extract scenes from, if null, all logs are extracted 12 | tokens: null # list of tokens to extract scenes from, if null, all tokens are extracted -------------------------------------------------------------------------------- /navsim/planning/script/config/common/default_common.yaml: -------------------------------------------------------------------------------- 1 | # Default common configs 2 | 3 | defaults: 4 | - scene_filter: all_scenes 5 | # Worker that is used to run simulations 6 | - worker: ray_distributed_no_torch 7 | 8 | split: ??? 9 | 10 | distributed_timeout_seconds: 7200 # Sets how long to wait while synchronizing across worker nodes in a distributed context. 11 | 12 | selected_simulation_metrics: null 13 | 14 | # Sets verbosity level, in particular determines if progress bars are shown or not. 15 | verbose: false 16 | 17 | # Logger 18 | logger_level: info # Level of logger 19 | logger_format_string: null # Logger format string, set null to use the default format string 20 | 21 | # Execution 22 | max_number_of_workers: null # Set null to disable threading for simulation execution 23 | gpu: true # Whether to use available GPUs during training/simulation -------------------------------------------------------------------------------- /docs/cache.md: -------------------------------------------------------------------------------- 1 | # Understanding the data format and classes 2 | 3 | OpenScene is a compact redistribution of the large-scale [nuPlan dataset](https://motional-nuplan.s3.ap-northeast-1.amazonaws.com/index.html), retaining only relevant annotations and sensor data at 2Hz. This reduces the dataset size by a factor of >10. The data used in NAVSIM is structured into `navsim.common.dataclasses.Scene` objects. A `Scene` is a list of `Frame` objects, each containing the required inputs and annotations for training a planning `Agent`. 4 | 5 | **Caching.** Evaluating planners involves significant preprocessing of the raw annotation data, including accessing the global map at each ´Frame´ and converting it into a local coordinate system. You can generate the cache with: 6 | ``` 7 | cd $NAVSIM_DEVKIT_ROOT/scripts/ 8 | ./run_metric_caching.sh 9 | ``` 10 | 11 | This will create the metric cache under `$NAVSIM_EXP_ROOT/metric_cache`, where `$NAVSIM_EXP_ROOT` is defined by the environment variable set during installation. 12 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import setuptools 4 | 5 | # Change directory to allow installation from anywhere 6 | script_folder = os.path.dirname(os.path.realpath(__file__)) 7 | os.chdir(script_folder) 8 | 9 | with open("requirements.txt") as f: 10 | requirements = f.read().splitlines() 11 | 12 | # Installs 13 | setuptools.setup( 14 | name="navsim", 15 | version="1.1.0", 16 | author="University of Tuebingen", 17 | author_email="kashyap.chitta@uni-tuebingen.de", 18 | description="NAVSIM: Data-Driven Non-Reactive Autonomous Vehicle Simulation and Benchmarking", 19 | url="https://github.com/autonomousvision/navsim", 20 | python_requires=">=3.9", 21 | packages=setuptools.find_packages(script_folder), 22 | package_dir={"": "."}, 23 | classifiers=[ 24 | "Programming Language :: Python :: 3.9", 25 | "Operating System :: OS Independent", 26 | "License :: Free for non-commercial use", 27 | ], 28 | license="apache-2.0", 29 | install_requires=requirements, 30 | ) 31 | -------------------------------------------------------------------------------- /docs/install.md: -------------------------------------------------------------------------------- 1 | # Data 2 | root 3 | ├── navsim (containing the devkit) 4 | ├── exp 5 | └── dataset 6 |    ├── maps 7 |    ├── navsim_logs 8 | | ├── test 9 | | ├── trainval 10 |    └── sensor_blobs 11 | ├── test 12 | ├── trainval 13 | └── extra_data/planning_vb 14 | ├── trajectory_anchors_256.npy 15 | ├── formatted_pdm_score_256.npy 16 | ``` 17 | Set the required environment variables, by adding the following to your `~/.bashrc` file 18 | Based on the structure above, the environment variables need to be defined as: 19 | ``` 20 | export NUPLAN_MAP_VERSION="nuplan-maps-v1.0" 21 | export NUPLAN_MAPS_ROOT="$HOME/navsim_workspace/dataset/maps" 22 | export NAVSIM_EXP_ROOT="$HOME/navsim_workspace/exp" 23 | export NAVSIM_DEVKIT_ROOT="$HOME/navsim_workspace/navsim" 24 | export OPENSCENE_DATA_ROOT="$HOME/navsim_workspace/dataset" 25 | ``` 26 | 27 | ### Install the navsim-devkit 28 | Finally, install navsim. 29 | To this end, create a new environment and install the required dependencies: 30 | ``` 31 | conda env create --name navsim -f environment.yml 32 | conda activate navsim 33 | pip install -e . 34 | ``` -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Logos Robotics Group 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /navsim/planning/script/run_metric_caching.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import hydra 3 | from omegaconf import DictConfig 4 | 5 | from nuplan.planning.script.builders.logging_builder import build_logger 6 | 7 | from navsim.planning.metric_caching.caching import cache_data 8 | from navsim.planning.script.builders.worker_pool_builder import build_worker 9 | 10 | 11 | logger = logging.getLogger(__name__) 12 | 13 | CONFIG_PATH = "config/metric_caching" 14 | CONFIG_NAME = "default_metric_caching" 15 | 16 | 17 | @hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME) 18 | def main(cfg: DictConfig) -> None: 19 | """ 20 | Main entrypoint for training/validation experiments. 21 | :param cfg: omegaconf dictionary 22 | """ 23 | # Configure logger 24 | build_logger(cfg) 25 | 26 | # Build worker 27 | worker = build_worker(cfg) 28 | 29 | # Precompute and cache all features 30 | logger.info("Starting Metric Caching...") 31 | if cfg.worker == "ray_distributed" and cfg.worker.use_distributed: 32 | raise AssertionError("ray in distributed mode will not work with this job") 33 | cache_data(cfg=cfg, worker=worker) 34 | 35 | if __name__ == "__main__": 36 | main() 37 | -------------------------------------------------------------------------------- /navsim/planning/script/config/pdm_scoring/default_scoring_parameters.yaml: -------------------------------------------------------------------------------- 1 | proposal_sampling: 2 | _target_: nuplan.planning.simulation.trajectory.trajectory_sampling.TrajectorySampling 3 | _convert_: 'all' 4 | num_poses: 40 5 | interval_length: 0.1 6 | 7 | simulator: 8 | _target_: navsim.planning.simulation.planner.pdm_planner.simulation.pdm_simulator.PDMSimulator 9 | _convert_: 'all' 10 | proposal_sampling: ${proposal_sampling} 11 | 12 | scorer: 13 | _target_: navsim.planning.simulation.planner.pdm_planner.scoring.pdm_scorer.PDMScorer 14 | _convert_: 'all' 15 | proposal_sampling: ${proposal_sampling} 16 | config: 17 | _target_: navsim.planning.simulation.planner.pdm_planner.scoring.pdm_scorer.PDMScorerConfig 18 | _convert_: 'all' 19 | # weighted metric weights 20 | progress_weight: 5.0 21 | ttc_weight: 5.0 22 | comfortable_weight: 2.0 23 | 24 | # thresholds 25 | driving_direction_horizon: 1.0 # [s] (driving direction) 26 | driving_direction_compliance_threshold: 2.0 # [m] (driving direction) 27 | driving_direction_violation_threshold: 6.0 # [m] (driving direction) 28 | stopped_speed_threshold: 5e-03 # [m/s] (ttc) 29 | progress_distance_threshold: 5.0 # [m] (progress) -------------------------------------------------------------------------------- /navsim/planning/script/builders/worker_pool_builder.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | from hydra.utils import instantiate 4 | from omegaconf import DictConfig 5 | 6 | from nuplan.planning.script.builders.utils.utils_type import is_target_type, validate_type 7 | from nuplan.planning.utils.multithreading.worker_pool import WorkerPool 8 | from nuplan.planning.utils.multithreading.worker_parallel import SingleMachineParallelExecutor 9 | from nuplan.planning.utils.multithreading.worker_sequential import Sequential 10 | 11 | logger = logging.getLogger(__name__) 12 | 13 | 14 | def build_worker(cfg: DictConfig) -> WorkerPool: 15 | """ 16 | Builds the worker. 17 | :param cfg: DictConfig. Configuration that is used to run the experiment. 18 | :return: Instance of WorkerPool. 19 | """ 20 | logger.info('Building WorkerPool...') 21 | worker: WorkerPool = ( 22 | instantiate(cfg.worker) 23 | if ( 24 | is_target_type(cfg.worker, SingleMachineParallelExecutor) 25 | or is_target_type(cfg.worker, Sequential) 26 | ) 27 | else instantiate(cfg.worker, output_dir=cfg.output_dir) 28 | ) 29 | validate_type(worker, WorkerPool) 30 | 31 | logger.info('Building WorkerPool...DONE!') 32 | return worker 33 | -------------------------------------------------------------------------------- /navsim/planning/metric_caching/metric_cache.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import lzma 4 | import pickle 5 | from dataclasses import dataclass 6 | 7 | from typing import List 8 | from pathlib import Path 9 | from nuplan.planning.simulation.trajectory.interpolated_trajectory import InterpolatedTrajectory 10 | from nuplan.common.actor_state.ego_state import EgoState 11 | 12 | from navsim.planning.simulation.planner.pdm_planner.observation.pdm_observation import ( 13 | PDMObservation, 14 | ) 15 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_path import PDMPath 16 | from navsim.planning.simulation.planner.pdm_planner.observation.pdm_occupancy_map import ( 17 | PDMDrivableMap, 18 | ) 19 | 20 | from nuplan.common.utils.io_utils import save_buffer 21 | 22 | 23 | @dataclass 24 | class MetricCache: 25 | 26 | file_path: Path 27 | trajectory: InterpolatedTrajectory 28 | ego_state: EgoState 29 | 30 | observation: PDMObservation 31 | centerline: PDMPath 32 | route_lane_ids: List[str] 33 | drivable_area_map: PDMDrivableMap 34 | 35 | def dump(self) -> None: 36 | # TODO: check if file_path must really be pickled 37 | pickle_object = pickle.dumps(self, protocol=pickle.HIGHEST_PROTOCOL) 38 | save_buffer(self.file_path, lzma.compress(pickle_object, preset=0)) 39 | -------------------------------------------------------------------------------- /navsim/agents/human_agent.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 3 | from navsim.agents.abstract_agent import AbstractAgent 4 | from navsim.common.dataclasses import AgentInput, Trajectory, Scene, SensorConfig 5 | 6 | class HumanAgent(AbstractAgent): 7 | 8 | requires_scene = True 9 | 10 | def __init__( 11 | self, 12 | trajectory_sampling: TrajectorySampling = TrajectorySampling( 13 | time_horizon=4, interval_length=0.5 14 | ), 15 | ): 16 | self._trajectory_sampling = trajectory_sampling 17 | 18 | def name(self) -> str: 19 | """Inherited, see superclass.""" 20 | 21 | return self.__class__.__name__ 22 | 23 | def initialize(self) -> None: 24 | """Inherited, see superclass.""" 25 | pass 26 | 27 | def get_sensor_config(self) -> SensorConfig: 28 | """Inherited, see superclass.""" 29 | return SensorConfig.build_no_sensors() 30 | 31 | def compute_trajectory(self, agent_input: AgentInput, scene: Scene) -> Trajectory: 32 | """ 33 | Computes the ego vehicle trajectory. 34 | :param current_input: Dataclass with agent inputs. 35 | :return: Trajectory representing the predicted ego's position in future 36 | """ 37 | return scene.get_future_trajectory(self._trajectory_sampling.num_poses) -------------------------------------------------------------------------------- /navsim/planning/training/abstract_feature_target_builder.py: -------------------------------------------------------------------------------- 1 | from abc import abstractmethod 2 | from typing import Dict 3 | 4 | from torch import Tensor 5 | 6 | from navsim.common.dataclasses import AgentInput, Scene 7 | 8 | class AbstractFeatureBuilder: 9 | def __init__(self): 10 | pass 11 | 12 | @abstractmethod 13 | def get_unique_name(self) -> str: 14 | pass 15 | 16 | @abstractmethod 17 | def compute_features(self, agent_input: AgentInput) -> Dict[str, Tensor]: 18 | """ 19 | Computes features from the AgentInput object, i.e., without access to ground-truth. 20 | Outputs a dictionary where each item has a unique identifier and maps to a single feature tensor. 21 | One FeatureBuilder can return a dict with multiple FeatureTensors. 22 | """ 23 | pass 24 | 25 | class AbstractTargetBuilder: 26 | def __init__(self): 27 | pass 28 | 29 | @abstractmethod 30 | def get_unique_name(self) -> str: 31 | pass 32 | 33 | @abstractmethod 34 | def compute_targets(self, scene: Scene) -> Dict[str, Tensor]: 35 | """ 36 | Computes targets from the Scene object, i.e., with access to ground-truth. 37 | Outputs a dictionary where each item has a unique identifier and maps to a single target tensor. 38 | One TargetBuilder can return a dict with multiple TargetTensors. 39 | """ 40 | pass -------------------------------------------------------------------------------- /navsim/planning/script/builders/planner_builder.py: -------------------------------------------------------------------------------- 1 | from typing import List, Optional, Type, cast 2 | 3 | from hydra._internal.utils import _locate 4 | from hydra.utils import instantiate 5 | from omegaconf import DictConfig 6 | 7 | from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario 8 | from nuplan.planning.simulation.planner.abstract_planner import AbstractPlanner 9 | 10 | 11 | def _build_planner(planner_cfg: DictConfig, scenario: Optional[AbstractScenario]) -> AbstractPlanner: 12 | """ 13 | Instantiate planner 14 | :param planner_cfg: config of a planner 15 | :param scenario: scenario 16 | :return AbstractPlanner 17 | """ 18 | config = planner_cfg.copy() 19 | 20 | planner_cls: Type[AbstractPlanner] = _locate(config._target_) 21 | 22 | if planner_cls.requires_scenario: 23 | assert scenario is not None, ( 24 | "Scenario was not provided to build the planner. " f"Planner {config} can not be build!" 25 | ) 26 | planner = cast(AbstractPlanner, instantiate(config, scenario=scenario)) 27 | else: 28 | planner = cast(AbstractPlanner, instantiate(config)) 29 | 30 | return planner 31 | 32 | 33 | def build_planners(planner_cfg: DictConfig, scenario: Optional[AbstractScenario]) -> List[AbstractPlanner]: 34 | """ 35 | Instantiate multiple planners by calling build_planner 36 | :param planners_cfg: planners config 37 | :param scenario: scenario 38 | :return planners: List of AbstractPlanners 39 | """ 40 | return [_build_planner(planner, scenario) for planner in planner_cfg.values()] 41 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # nuplan-devkit @ git+https://github.com/motional/nuplan-devkit/@nuplan-devkit-v1.2 2 | scikit-learn==1.2.2 3 | positional-encodings==6.0.1 4 | 5 | # nuplan requirements 6 | aioboto3 7 | aiofiles 8 | bokeh==2.4.3 # Used in the nuBoard dashboard 9 | casadi # Used for optimization solving 10 | control==0.9.1 # Used for LQR controller synthesis 11 | Fiona # Used in GpkgMapsDB.py 12 | geopandas>=0.12.1 # Used to read maps 13 | guppy3==3.1.2 14 | hydra-core==1.2.0 # Used for configs 15 | joblib 16 | matplotlib # Used for rendering 17 | nest_asyncio # Used to nest event loops when running nuBoard/jupyter 18 | numpy==1.23.4 19 | opencv-python==4.9.0.80 # Used widely 20 | pandas # Used widely 21 | Pillow # Used widely to render images 22 | psutil # Used widely to get the number of CPUs 23 | pyarrow # For parquet 24 | pyinstrument # Used widely as profiler 25 | pyogrio # A performant backend for geopandas 26 | pyquaternion>=0.9.5 # Used widely, avoid known bug with quaternion slerp 27 | pytest # Used widely 28 | rasterio # Used in GpkgMapsDB.py 29 | ray # Used widely 30 | retry 31 | rtree # Used in occupancy maps 32 | scipy # Used widely 33 | selenium # Used in bokeh export png 34 | setuptools==65.5.1 # Used in setup.py, pinned to not break pytorch 35 | Shapely>=2.0.0 # Used widely 36 | SQLAlchemy==1.4.27 # older versions don't work with some table definitions 37 | sympy # Use for symbolic algebra 38 | tornado # Used in nuboard.py 39 | tqdm # Used widely 40 | ujson # Used in serialiation_callback.py 41 | 42 | torch==2.0.1 43 | torchvision==0.15.2 44 | pytorch-lightning==2.2.1 45 | tensorboard==2.16.2 46 | protobuf==4.25.3 47 | 48 | notebook 49 | timm 50 | -------------------------------------------------------------------------------- /navsim/planning/metric_caching/metric_caching_utils.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from typing import Any, Tuple, Type, Union 4 | 5 | import numpy as np 6 | import numpy.typing as npt 7 | 8 | from scipy.interpolate import interp1d 9 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_geometry_utils import ( 10 | normalize_angle, 11 | ) 12 | 13 | 14 | class StateInterpolator: 15 | 16 | def __init__(self, state_array: npt.NDArray[np.float64]): 17 | 18 | # attribute 19 | self._state_array = state_array 20 | 21 | # loaded during initialization 22 | self._time = state_array[:, 0] 23 | self._states = state_array[:, 1:] 24 | 25 | # unwrap heading angle 26 | self._states[:, 2] = np.unwrap(self._states[:, 2], axis=0) 27 | self._interpolator = interp1d(self._time, self._states, axis=0) 28 | 29 | def __reduce__(self) -> Tuple[Type[StateInterpolator], Tuple[Any, ...]]: 30 | """Helper for pickling.""" 31 | return self.__class__, (self.state_array,) 32 | 33 | @property 34 | def start_time(self): 35 | return self._time[0] 36 | 37 | @property 38 | def end_time(self): 39 | return self._time[-1] 40 | 41 | def interpolate( 42 | self, 43 | time: float, 44 | ) -> Union[npt.NDArray[np.object_], npt.NDArray[np.float64]]: 45 | 46 | if self.start_time <= time <= self.end_time: 47 | 48 | interpolated_state = self._interpolator(time) 49 | interpolated_state[2] = normalize_angle(interpolated_state[2]) 50 | return interpolated_state 51 | 52 | return None 53 | -------------------------------------------------------------------------------- /navsim/planning/training/callbacks/time_logging_callback.py: -------------------------------------------------------------------------------- 1 | import time 2 | from typing import Any, Optional 3 | 4 | import pytorch_lightning as pl 5 | 6 | 7 | class TimeLoggingCallback(pl.Callback): 8 | def __init__(self) -> None: 9 | pass 10 | 11 | def on_validation_epoch_start(self, trainer: pl.Trainer, lightning_module: pl.LightningModule) -> None: 12 | self.val_start = time.time() 13 | 14 | def on_validation_epoch_end(self, trainer: pl.Trainer, lightning_module: pl.LightningModule) -> None: 15 | lightning_module.log_dict( 16 | { 17 | 'time_eval': time.time() - self.val_start, 18 | 'step': lightning_module.current_epoch, 19 | } 20 | ) 21 | 22 | def on_test_epoch_start(self, trainer: pl.Trainer, lightning_module: pl.LightningModule) -> None: 23 | self.test_start = time.time() 24 | 25 | def on_test_epoch_end(self, trainer: pl.Trainer, lightning_module: pl.LightningModule) -> None: 26 | lightning_module.log_dict( 27 | { 28 | 'time_test': time.time() - self.test_start, 29 | 'step': lightning_module.current_epoch, 30 | } 31 | ) 32 | 33 | def on_train_epoch_start(self, trainer: pl.Trainer, lightning_module: pl.LightningModule) -> None: 34 | self.train_start = time.time() 35 | 36 | def on_train_epoch_end( 37 | self, trainer: pl.Trainer, lightning_module: pl.LightningModule, unused: Optional[Any] = None 38 | ) -> None: 39 | lightning_module.log_dict( 40 | { 41 | 'time_epoch': time.time() - self.train_start, 42 | 'step': lightning_module.current_epoch, 43 | } 44 | ) 45 | -------------------------------------------------------------------------------- /docs/metrics.md: -------------------------------------------------------------------------------- 1 | # Understanding the PDM Score 2 | 3 | Fair comparisons are challenging in the open-loop planning literature, due to metrics of narrow scope or inconsistent definitions between different projects. The PDM Score is a combination of six sub-metrics, which provides a comprehensive analysis of different aspects of driving performance. Five of these sub-metrics are discrete-valued, and one is continuous. All metrics are computed after a 4-second non-reactive simulation of the planner output: background actors follow their recorded future trajectories, and the ego vehicle moves based on an LQR controller. The full composition of the PDM score is detailed below: 4 | 5 | Metric | Weight | Range | 6 | |---|---|---| 7 | No at-fault Collisions (NC) | multiplier | {0, 1/2, 1} | 8 | Drivable Area Compliance (DAC) | multiplier | {0, 1} | 9 | Driving Direction Compliance (DDC) | multiplier | {0, 1/2, 1} | 10 | Time to Collision (TTC) within bound | 5 | {0, 1} | 11 | Comfort (C) | 2 | {0, 1} | 12 | Ego Progress (EP) | 5 | [0, 1] | 13 | 14 | i.e., `PDM Score = NC * DAC * DDC * (5*TTC + 2*C + 5*EP) / 12` 15 | 16 | To evaluate the PDM score for an agent you can run: 17 | ``` 18 | cd $NAVSIM_DEVKIT_ROOT/scripts/ 19 | ./run_cv_pdm_score_evaluation.sh 20 | ``` 21 | 22 | By default, this will generate an evaluation csv for a simple constant velocity [planning baseline](https://github.com/autonomousvision/navsim/blob/main/docs/agents.md#output). You can modify the script to evaluate your own planning agent. 23 | 24 | For instance, you can add a new config for your agent under `$NAVSIM_DEVKIT_ROOT/navsim/navsim/planning/script/config/pdm_scoring/agent/my_new_agent.yaml`. 25 | Then, running your own agent is as simple as adding an override `agent=my_new_agent` to the script. 26 | You can find an example in `run_human_agent_pdm_score_evaluation.sh` 27 | -------------------------------------------------------------------------------- /navsim/agents/constant_velocity_agent.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 3 | from navsim.agents.abstract_agent import AbstractAgent 4 | from navsim.common.dataclasses import AgentInput, Trajectory, SensorConfig 5 | 6 | import numpy as np 7 | 8 | 9 | class ConstantVelocityAgent(AbstractAgent): 10 | 11 | requires_scene = False 12 | 13 | def __init__( 14 | self, 15 | trajectory_sampling: TrajectorySampling = TrajectorySampling( 16 | time_horizon=4, interval_length=0.5 17 | ), 18 | ): 19 | self._trajectory_sampling = trajectory_sampling 20 | 21 | def name(self) -> str: 22 | """Inherited, see superclass.""" 23 | 24 | return self.__class__.__name__ 25 | 26 | def initialize(self) -> None: 27 | """Inherited, see superclass.""" 28 | pass 29 | 30 | def get_sensor_config(self) -> SensorConfig: 31 | """Inherited, see superclass.""" 32 | return SensorConfig.build_no_sensors() 33 | 34 | def compute_trajectory(self, agent_input: AgentInput) -> Trajectory: 35 | """ 36 | Computes the ego vehicle trajectory. 37 | :param current_input: Dataclass with agent inputs. 38 | :return: Trajectory representing the predicted ego's position in future 39 | """ 40 | ego_velocity_2d = agent_input.ego_statuses[-1].ego_velocity 41 | ego_speed = (ego_velocity_2d**2).sum(-1) ** 0.5 42 | 43 | num_poses, dt = ( 44 | self._trajectory_sampling.num_poses, 45 | self._trajectory_sampling.interval_length, 46 | ) 47 | poses = np.array( 48 | [[(time_idx + 1) * dt * ego_speed, 0.0, 0.0] for time_idx in range(num_poses)], 49 | dtype=np.float32, 50 | ) 51 | 52 | return Trajectory(poses, self._trajectory_sampling) 53 | -------------------------------------------------------------------------------- /navsim/planning/training/agent_lightning_module.py: -------------------------------------------------------------------------------- 1 | import pytorch_lightning as pl 2 | from torch import Tensor 3 | from typing import Dict, Tuple 4 | import torch 5 | from navsim.agents.abstract_agent import AbstractAgent 6 | 7 | class AgentLightningModule(pl.LightningModule): 8 | def __init__( 9 | self, 10 | agent: AbstractAgent, 11 | ): 12 | super().__init__() 13 | self.agent = agent 14 | 15 | def _step( 16 | self, 17 | batch: Tuple[Dict[str, Tensor], Dict[str, Tensor]], 18 | logging_prefix: str, 19 | ): 20 | features, targets = batch 21 | 22 | input_target = self.agent.config.input_target if hasattr(self.agent.config, 'input_target') else False 23 | if input_target: 24 | prediction = self.agent.forward(features, targets) 25 | else: 26 | prediction = self.agent.forward(features) 27 | 28 | loss_dict = self.agent.compute_loss(features, targets, prediction) 29 | if isinstance(loss_dict, Tensor): 30 | loss_dict = {"traj_loss": loss_dict} 31 | 32 | total_loss = 0.0 33 | for loss_key, loss_value in loss_dict.items(): 34 | self.log(f"{logging_prefix}_{loss_key}", loss_value, on_step=True, on_epoch=True, prog_bar=True, sync_dist=True) 35 | if 'acc' in loss_key: 36 | continue 37 | total_loss = total_loss + loss_value 38 | self.log(f"{logging_prefix}_total_loss", total_loss, on_step=True, on_epoch=True, prog_bar=True, sync_dist=True) 39 | return total_loss 40 | 41 | def training_step( 42 | self, 43 | batch: Tuple[Dict[str, Tensor], Dict[str, Tensor]], 44 | batch_idx: int 45 | ): 46 | return self._step(batch, "train") 47 | 48 | def validation_step( 49 | self, 50 | batch: Tuple[Dict[str, Tensor], Dict[str, Tensor]], 51 | batch_idx: int 52 | ): 53 | return self._step(batch, "val") 54 | 55 | def configure_optimizers(self): 56 | return self.agent.get_optimizers() 57 | 58 | # def backward(self, loss): 59 | # # print('set detect anomaly') 60 | # # torch.autograd.set_detect_anomaly(True) 61 | # loss.backward(retain_graph=True) 62 | -------------------------------------------------------------------------------- /navsim/planning/script/config/training/default_training.yaml: -------------------------------------------------------------------------------- 1 | hydra: 2 | run: 3 | dir: ${output_dir} 4 | output_subdir: ${output_dir}/code/hydra # Store hydra's config breakdown here for debugging 5 | searchpath: # Only in these paths are discoverable 6 | - pkg://navsim.planning.script.config.common 7 | # - pkg://navsim.planning.script.config.pdm_scoring 8 | # - pkg://navsim.planning.script.config.training 9 | 10 | defaults: 11 | - default_common 12 | - default_evaluation 13 | - default_train_val_test_log_split 14 | - agent: ego_status_mlp_agent 15 | 16 | split: trainval 17 | # cache_path: ${oc.env:NAVSIM_EXP_ROOT}/training_cache 18 | cache_path: '' #no cache 19 | use_cache_without_dataset: false # load the training samples from the cache. scene-filter will be ignored 20 | force_cache_computation: false 21 | 22 | dataloader: 23 | params: 24 | batch_size: 32 # number of samples per batch 25 | num_workers: 4 # number of workers for data loading 26 | pin_memory: true # pin memory for faster GPU transfer 27 | prefetch_factor: 2 # number of samples loaded in advance by each worker 28 | 29 | trainer: 30 | params: 31 | max_epochs: 20 # maximum number of training epochs 32 | check_val_every_n_epoch: 200 # run validation set every n training epochs 33 | val_check_interval: 1.0 # [%] run validation set every X% of training set 34 | 35 | limit_train_batches: 1.0 # how much of training dataset to check (float = fraction, int = num_batches) 36 | limit_val_batches: 1.0 # how much of validation dataset to check (float = fraction, int = num_batches) 37 | 38 | accelerator: gpu # distribution method 39 | strategy: ddp 40 | precision: 16-mixed # floating point precision 41 | num_nodes: 1 # Number of nodes used for training 42 | 43 | num_sanity_val_steps: 0 # number of validation steps to run before training begins 44 | fast_dev_run: false # runs 1 batch of train/val/test for sanity 45 | 46 | accumulate_grad_batches: 1 # accumulates gradients every n batches 47 | # track_grad_norm: -1 # logs the p-norm for inspection 48 | gradient_clip_val: 0.0 # value to clip gradients 49 | gradient_clip_algorithm: norm # [value, norm] method to clip gradients -------------------------------------------------------------------------------- /docs/submission.md: -------------------------------------------------------------------------------- 1 | # Submitting to the Leaderboard 2 | 3 | NAVSIM comes with official leaderboards on HuggingFace. The leaderboards prevent ambiguity in metric definitions between different projects, as all evaluation is performed on the server with the official evaluation script. 4 | 5 | To submit to a leaderboard you need to create a pickle file that contains a trajectory for each test scenario. NAVSIM provides a script to create such a pickle file. 6 | 7 | Have a look at `run_create_submission_pickle.sh`: this file creates the pickle file for the ConstantVelocity agent. You can run it for your own agent by replacing the `agent` override. 8 | Follow the [submission instructions on huggingface](https://huggingface.co/spaces/AGC2024-P/e2e-driving-2024) to upload your submission. 9 | **Note that you have to set the variables `TEAM_NAME`, `AUTHORS`, `EMAIL`, `INSTITUTION`, and `COUNTRY` in `run_create_submission_pickle.sh` to generate a valid submission file** 10 | 11 | ### Warm-up track 12 | The warm-up track evaluates your submission on a [warm-up leaderboard](https://huggingface.co/spaces/AGC2024-P/e2e-driving-warmup) based on the `mini` split. This allows you to test your method and get familiar with the devkit and the submission procedure, with a less restrictive submission budget (up to 5 submissions daily). Instructions on making a submission on HuggingFace are available in the HuggingFace space. Performance on the warm-up leaderboard is not taken into consideration for determining your team's ranking for the 2024 Autonomous Grand Challenge. 13 | Use the script `run_create_submission_pickle_warmup.sh` which already contains the overrides `scene_filter=warmup_test_e2e` and `split=mini` to generate the submission file for the warmup track. 14 | 15 | You should be able to obtain the same evaluation results as on the server, by running the evaluation locally. 16 | To do so, use the overrides `scene_filter=warmup_test_e2e` when executing the script to run the PDM scoring (e.g., `run_cv_pdm_score_evaluation.sh` for the constant-velocity agent). 17 | 18 | ### Formal track 19 | This is the [official challenge leaderboard](https://huggingface.co/spaces/AGC2024-P/e2e-driving-2024), based on secret held-out test frames (see submission_test split on the install page). 20 | Use the script `run_create_submission_pickle.sh`. It will by default run with `scene_filter=private_test_e2e` and `split=private_test_e2e`. 21 | You only need to set your own agent with the `agent` override. 22 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/scoring/pdm_scorer_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.typing as npt 3 | from nuplan.common.actor_state.state_representation import StateSE2 4 | from nuplan.common.actor_state.tracked_objects import TrackedObject 5 | from nuplan.planning.metrics.utils.collision_utils import CollisionType 6 | from nuplan.planning.simulation.observation.idm.utils import ( 7 | is_agent_behind, 8 | is_track_stopped, 9 | ) 10 | from shapely import LineString, Polygon 11 | 12 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_enums import ( 13 | StateIndex, 14 | ) 15 | 16 | 17 | def get_collision_type( 18 | state: npt.NDArray[np.float64], 19 | ego_polygon: Polygon, 20 | tracked_object: TrackedObject, 21 | tracked_object_polygon: Polygon, 22 | stopped_speed_threshold: float = 5e-02, 23 | ) -> CollisionType: 24 | """ 25 | Classify collision between ego and the track. 26 | :param ego_state: Ego's state at the current timestamp. 27 | :param tracked_object: Tracked object. 28 | :param stopped_speed_threshold: Threshold for 0 speed due to noise. 29 | :return Collision type. 30 | """ 31 | 32 | ego_speed = np.hypot( 33 | state[StateIndex.VELOCITY_X], 34 | state[StateIndex.VELOCITY_Y], 35 | ) 36 | 37 | is_ego_stopped = float(ego_speed) <= stopped_speed_threshold 38 | 39 | center_point = tracked_object_polygon.centroid 40 | tracked_object_center = StateSE2( 41 | center_point.x, center_point.y, tracked_object.box.center.heading 42 | ) 43 | 44 | ego_rear_axle_pose: StateSE2 = StateSE2(*state[StateIndex.STATE_SE2]) 45 | 46 | # Collisions at (close-to) zero ego speed 47 | if is_ego_stopped: 48 | collision_type = CollisionType.STOPPED_EGO_COLLISION 49 | 50 | # Collisions at (close-to) zero track speed 51 | elif is_track_stopped(tracked_object): 52 | collision_type = CollisionType.STOPPED_TRACK_COLLISION 53 | 54 | # Rear collision when both ego and track are not stopped 55 | elif is_agent_behind(ego_rear_axle_pose, tracked_object_center): 56 | collision_type = CollisionType.ACTIVE_REAR_COLLISION 57 | 58 | # Front bumper collision when both ego and track are not stopped 59 | elif LineString( 60 | [ 61 | ego_polygon.exterior.coords[0], 62 | ego_polygon.exterior.coords[3], 63 | ] 64 | ).intersects(tracked_object_polygon): 65 | collision_type = CollisionType.ACTIVE_FRONT_COLLISION 66 | 67 | # Lateral collision when both ego and track are not stopped 68 | else: 69 | collision_type = CollisionType.ACTIVE_LATERAL_COLLISION 70 | 71 | return collision_type 72 | -------------------------------------------------------------------------------- /navsim/visualization/lidar.py: -------------------------------------------------------------------------------- 1 | from typing import Any, List 2 | 3 | import numpy as np 4 | import numpy.typing as npt 5 | 6 | import matplotlib 7 | from matplotlib import pyplot as plt 8 | 9 | from navsim.visualization.config import LIDAR_CONFIG 10 | from navsim.common.enums import LidarIndex 11 | 12 | 13 | def filter_lidar_pc(lidar_pc: npt.NDArray[np.float32]) -> npt.NDArray[np.float32]: 14 | """ 15 | Filter lidar point cloud according to global configuration 16 | :param lidar_pc: numpy array of shape (6,n) 17 | :return: filtered point cloud 18 | """ 19 | 20 | pc = lidar_pc.T 21 | mask = ( 22 | np.ones((len(pc)), dtype=bool) 23 | & (pc[:, LidarIndex.X] > LIDAR_CONFIG["x_lim"][0]) 24 | & (pc[:, LidarIndex.X] < LIDAR_CONFIG["x_lim"][1]) 25 | & (pc[:, LidarIndex.Y] > LIDAR_CONFIG["y_lim"][0]) 26 | & (pc[:, LidarIndex.Y] < LIDAR_CONFIG["y_lim"][1]) 27 | & (pc[:, LidarIndex.Z] > LIDAR_CONFIG["z_lim"][0]) 28 | & (pc[:, LidarIndex.Z] < LIDAR_CONFIG["z_lim"][1]) 29 | ) 30 | pc = pc[mask] 31 | return pc.T 32 | 33 | 34 | def get_lidar_pc_color( 35 | lidar_pc: npt.NDArray[np.float32], as_hex: bool = False 36 | ) -> List[Any]: 37 | """ 38 | Compute color map of lidar point cloud according to global configuration 39 | :param lidar_pc: numpy array of shape (6,n) 40 | :param as_hex: whether to return hex values, defaults to False 41 | :return: list of RGB or hex values 42 | """ 43 | 44 | pc = lidar_pc.T 45 | if LIDAR_CONFIG["color_element"] == "none": 46 | colors_rgb = np.zeros((len(pc), 3), dtype=np.uin8) 47 | 48 | else: 49 | if LIDAR_CONFIG["color_element"] == "distance": 50 | color_intensities = np.linalg.norm(pc[:, LidarIndex.POSITION], axis=-1) 51 | else: 52 | color_element_map = { 53 | "x": LidarIndex.X, 54 | "y": LidarIndex.Y, 55 | "z": LidarIndex.Z, 56 | "intensity": LidarIndex.INTENSITY, 57 | "ring": LidarIndex.RING, 58 | "id": LidarIndex.ID, 59 | } 60 | color_intensities = pc[:, color_element_map[LIDAR_CONFIG["color_element"]]] 61 | 62 | min, max = color_intensities.min(), color_intensities.max() 63 | norm_intensities = [(value - min) / (max - min) for value in color_intensities] 64 | colormap = plt.get_cmap("viridis") 65 | colors_rgb = np.array([colormap(value) for value in norm_intensities]) 66 | colors_rgb = (colors_rgb[:, :3] * 255).astype(np.uint8) 67 | 68 | assert len(colors_rgb) == len(pc) 69 | if as_hex: 70 | return [matplotlib.colors.to_hex(tuple(c / 255.0 for c in rgb)) for rgb in colors_rgb] 71 | 72 | return [tuple(value) for value in colors_rgb] 73 | -------------------------------------------------------------------------------- /navsim/agents/transfuser/transfuser_agent.py: -------------------------------------------------------------------------------- 1 | from typing import Any, List, Dict, Union 2 | 3 | import torch 4 | from torch.optim import Optimizer 5 | from torch.optim.lr_scheduler import LRScheduler 6 | import pytorch_lightning as pl 7 | 8 | from navsim.agents.abstract_agent import AbstractAgent 9 | from navsim.common.dataclasses import SensorConfig 10 | from navsim.planning.training.abstract_feature_target_builder import ( 11 | AbstractFeatureBuilder, 12 | AbstractTargetBuilder, 13 | ) 14 | from navsim.agents.transfuser.transfuser_config import TransfuserConfig 15 | from navsim.agents.transfuser.transfuser_model import TransfuserModel 16 | from navsim.agents.transfuser.transfuser_callback import TransfuserCallback 17 | from navsim.agents.transfuser.transfuser_loss import transfuser_loss 18 | from navsim.agents.transfuser.transfuser_features import ( 19 | TransfuserFeatureBuilder, 20 | TransfuserTargetBuilder, 21 | ) 22 | 23 | 24 | class TransfuserAgent(AbstractAgent): 25 | def __init__( 26 | self, 27 | config: TransfuserConfig, 28 | lr: float, 29 | checkpoint_path: str = None, 30 | ): 31 | super().__init__() 32 | 33 | self._config = config 34 | self._lr = lr 35 | 36 | self._checkpoint_path = checkpoint_path 37 | self._transfuser_model = TransfuserModel(config) 38 | 39 | def name(self) -> str: 40 | """Inherited, see superclass.""" 41 | 42 | return self.__class__.__name__ 43 | 44 | def initialize(self) -> None: 45 | """Inherited, see superclass.""" 46 | if torch.cuda.is_available(): 47 | state_dict: Dict[str, Any] = torch.load(self._checkpoint_path)["state_dict"] 48 | else: 49 | state_dict: Dict[str, Any] = torch.load(self._checkpoint_path, map_location=torch.device("cpu"))["state_dict"] 50 | self.load_state_dict({k.replace("agent.", ""): v for k, v in state_dict.items()}) 51 | 52 | def get_sensor_config(self) -> SensorConfig: 53 | """Inherited, see superclass.""" 54 | return SensorConfig.build_all_sensors(include=[3]) 55 | 56 | def get_target_builders(self) -> List[AbstractTargetBuilder]: 57 | return [TransfuserTargetBuilder(config=self._config)] 58 | 59 | def get_feature_builders(self) -> List[AbstractFeatureBuilder]: 60 | return [TransfuserFeatureBuilder(config=self._config)] 61 | 62 | def forward(self, features: Dict[str, torch.Tensor]) -> Dict[str, torch.Tensor]: 63 | return self._transfuser_model(features) 64 | 65 | def compute_loss( 66 | self, 67 | features: Dict[str, torch.Tensor], 68 | targets: Dict[str, torch.Tensor], 69 | predictions: Dict[str, torch.Tensor], 70 | ) -> torch.Tensor: 71 | return transfuser_loss(targets, predictions, self._config) 72 | 73 | def get_optimizers(self) -> Union[Optimizer, Dict[str, Union[Optimizer, LRScheduler]]]: 74 | return torch.optim.Adam(self._transfuser_model.parameters(), lr=self._lr) 75 | 76 | def get_training_callbacks(self) -> List[pl.Callback]: 77 | return [TransfuserCallback(self._config)] 78 | -------------------------------------------------------------------------------- /navsim/planning/script/run_dataset_caching.py: -------------------------------------------------------------------------------- 1 | import hydra 2 | from hydra.utils import instantiate 3 | import logging 4 | from omegaconf import DictConfig 5 | import os 6 | from pathlib import Path 7 | import pytorch_lightning as pl 8 | from typing import Any, Dict, List, Optional, Union 9 | import uuid 10 | 11 | from navsim.planning.training.dataset import Dataset 12 | from navsim.common.dataloader import SceneLoader 13 | from navsim.common.dataclasses import SceneFilter, SensorConfig 14 | from navsim.agents.abstract_agent import AbstractAgent 15 | 16 | from nuplan.planning.utils.multithreading.worker_pool import WorkerPool 17 | from nuplan.planning.utils.multithreading.worker_utils import worker_map 18 | 19 | logger = logging.getLogger(__name__) 20 | 21 | CONFIG_PATH = "config/training" 22 | CONFIG_NAME = "default_training" 23 | 24 | def cache_features(args: List[Dict[str, Union[List[str], DictConfig]]]) -> List[Optional[Any]]: 25 | node_id = int(os.environ.get("NODE_RANK", 0)) 26 | thread_id = str(uuid.uuid4()) 27 | 28 | log_names = [a["log_file"] for a in args] 29 | tokens = [t for a in args for t in a["tokens"]] 30 | cfg: DictConfig = args[0]["cfg"] 31 | 32 | agent: AbstractAgent = instantiate(cfg.agent) 33 | 34 | scene_filter: SceneFilter =instantiate(cfg.scene_filter) 35 | scene_filter.log_names = log_names 36 | scene_filter.tokens = tokens 37 | scene_loader = SceneLoader( 38 | sensor_blobs_path=Path(cfg.sensor_blobs_path), 39 | data_path=Path(cfg.navsim_log_path), 40 | scene_filter=scene_filter, 41 | sensor_config=agent.get_sensor_config(), 42 | ) 43 | logger.info( 44 | f"Extracted {len(scene_loader.tokens)} scenarios for thread_id={thread_id}, node_id={node_id}." 45 | ) 46 | 47 | 48 | dataset = Dataset( 49 | scene_loader=scene_loader, 50 | feature_builders=agent.get_feature_builders(), 51 | target_builders=agent.get_target_builders(), 52 | cache_path=cfg.cache_path, 53 | force_cache_computation=cfg.force_cache_computation, 54 | ) 55 | return [] 56 | 57 | 58 | @hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME) 59 | def main(cfg: DictConfig) -> None: 60 | logger.info("Global Seed set to 0") 61 | pl.seed_everything(0, workers=True) 62 | 63 | logger.info("Building Worker") 64 | worker: WorkerPool = instantiate(cfg.worker) 65 | 66 | logger.info("Building SceneLoader") 67 | scene_filter: SceneFilter = instantiate(cfg.scene_filter) 68 | data_path = Path(cfg.navsim_log_path) 69 | sensor_blobs_path = Path(cfg.sensor_blobs_path) 70 | scene_loader = SceneLoader( 71 | sensor_blobs_path=sensor_blobs_path, 72 | data_path=data_path, 73 | scene_filter=scene_filter, 74 | sensor_config=SensorConfig.build_no_sensors(), 75 | ) 76 | logger.info(f"Extracted {len(scene_loader)} scenarios for training/validation dataset") 77 | 78 | data_points = [ 79 | { 80 | "cfg": cfg, 81 | "log_file": log_file, 82 | "tokens": tokens_list, 83 | } 84 | for log_file, tokens_list in scene_loader.get_tokens_list_per_log().items() 85 | ] 86 | 87 | _ = worker_map(worker, cache_features, data_points) 88 | 89 | logger.info(f"Finished caching {len(scene_loader)} scenarios for training/validation dataset") 90 | 91 | if __name__ == "__main__": 92 | main() -------------------------------------------------------------------------------- /navsim/planning/script/run_create_submission_pickle.py: -------------------------------------------------------------------------------- 1 | from tqdm import tqdm 2 | import traceback 3 | import pickle 4 | import hydra 5 | from hydra.utils import instantiate 6 | from omegaconf import DictConfig 7 | import os 8 | 9 | from pathlib import Path 10 | from typing import Dict 11 | import logging 12 | 13 | from navsim.agents.abstract_agent import AbstractAgent 14 | from navsim.common.dataclasses import Trajectory, SceneFilter 15 | from navsim.common.dataloader import SceneLoader 16 | 17 | 18 | logger = logging.getLogger(__name__) 19 | 20 | CONFIG_PATH = "config/pdm_scoring" 21 | CONFIG_NAME = "default_run_create_submission_pickle" 22 | 23 | 24 | @hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME) 25 | def main(cfg: DictConfig) -> None: 26 | agent = instantiate(cfg.agent) 27 | data_path = Path(cfg.navsim_log_path) 28 | sensor_blobs_path = Path(cfg.sensor_blobs_path) 29 | save_path = Path(cfg.output_dir) 30 | scene_filter = instantiate(cfg.scene_filter) 31 | 32 | output = run_test_evaluation( 33 | agent=agent, 34 | scene_filter=scene_filter, 35 | data_path=data_path, 36 | sensor_blobs_path=sensor_blobs_path, 37 | ) 38 | 39 | submission = { 40 | "team_name": cfg.team_name, 41 | "authors": cfg.authors, 42 | "email": cfg.email, 43 | "institution": cfg.institution, 44 | "country / region": cfg.country, 45 | "predictions": output, 46 | } 47 | 48 | # pickle and save dict 49 | filename = os.path.join(save_path, "submission.pkl") 50 | with open(filename, 'wb') as file: 51 | pickle.dump(submission, file) 52 | logger.info(f"Your submission filed was saved to {filename}") 53 | 54 | def run_test_evaluation( 55 | agent: AbstractAgent, 56 | scene_filter: SceneFilter, 57 | data_path: Path, 58 | sensor_blobs_path: Path, 59 | ) -> Dict[str, Trajectory]: 60 | """ 61 | Function to create the output file for evaluation of an agent on the testserver 62 | :param agent: Agent object 63 | :param data_path: pathlib path to navsim logs 64 | :param sensor_blobs_path: pathlib path to sensor blobs 65 | :param save_path: pathlib path to folder where scores are stored as .csv 66 | """ 67 | if agent.requires_scene: 68 | raise ValueError( 69 | """ 70 | In evaluation, no access to the annotated scene is provided, but only to the AgentInput. 71 | Thus, agent.requires_scene has to be False for the agent that is to be evaluated. 72 | """ 73 | ) 74 | logger.info("Building Agent Input Loader") 75 | input_loader = SceneLoader( 76 | data_path=data_path, 77 | scene_filter=scene_filter, 78 | sensor_blobs_path=sensor_blobs_path, 79 | sensor_config=agent.get_sensor_config() 80 | ) 81 | agent.initialize() 82 | 83 | output: Dict[str, Trajectory] = {} 84 | for token in tqdm(input_loader, desc="Running evaluation"): 85 | try: 86 | agent_input = input_loader.get_agent_input_from_token(token) 87 | trajectory = agent.compute_trajectory(agent_input) 88 | output.update({token: trajectory}) 89 | except Exception as e: 90 | logger.warning(f"----------- Agent failed for token {token}:") 91 | traceback.print_exc() 92 | 93 | return output 94 | 95 | if __name__ == "__main__": 96 | main() 97 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/proposal/pdm_proposal.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | from typing import List 3 | 4 | from shapely.geometry import LineString 5 | 6 | from navsim.planning.simulation.planner.pdm_planner.proposal.batch_idm_policy import ( 7 | BatchIDMPolicy, 8 | ) 9 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_path import PDMPath 10 | 11 | 12 | @dataclass 13 | class PDMProposal: 14 | """Dataclass for storing proposal information.""" 15 | 16 | proposal_idx: int 17 | lateral_idx: int 18 | longitudinal_idx: int 19 | path: PDMPath 20 | 21 | @property 22 | def linestring(self) -> LineString: 23 | """Getter for linestring of proposal's path.""" 24 | return self.path.linestring 25 | 26 | @property 27 | def length(self): 28 | """Getter for length [m] of proposal's path.""" 29 | return self.path.length 30 | 31 | 32 | class PDMProposalManager: 33 | """Class to store and manage lateral and longitudinal combination of proposals.""" 34 | 35 | def __init__( 36 | self, 37 | lateral_proposals: List[PDMPath], 38 | longitudinal_policies: BatchIDMPolicy, 39 | ): 40 | """ 41 | Constructor for PDMProposalManager 42 | :param lateral_proposals: list of path's to follow 43 | :param longitudinal_policies: IDM policy class (batch-wise) 44 | """ 45 | 46 | self._num_lateral_proposals: int = len(lateral_proposals) 47 | self._num_longitudinal_proposals: int = longitudinal_policies.num_policies 48 | self._longitudinal_policies: BatchIDMPolicy = longitudinal_policies 49 | 50 | self._proposals: List[PDMProposal] = [] 51 | proposal_idx = 0 52 | 53 | for lateral_idx in range(self._num_lateral_proposals): 54 | for longitudinal_idx in range(self._num_longitudinal_proposals): 55 | self._proposals.append( 56 | PDMProposal( 57 | proposal_idx=proposal_idx, 58 | lateral_idx=lateral_idx, 59 | longitudinal_idx=longitudinal_idx, 60 | path=lateral_proposals[lateral_idx], 61 | ) 62 | ) 63 | proposal_idx += 1 64 | 65 | def __len__(self) -> int: 66 | """Returns number of proposals (paths x policies).""" 67 | return len(self._proposals) 68 | 69 | def __getitem__(self, proposal_idx) -> PDMProposal: 70 | """ 71 | Returns the requested proposal. 72 | :param proposal_idx: index for each proposal 73 | :return: PDMProposal dataclass 74 | """ 75 | return self._proposals[proposal_idx] 76 | 77 | def update(self, speed_limit_mps: float) -> None: 78 | """ 79 | Updates target velocities of IDM policies with current speed-limit. 80 | :param speed_limit_mps: current speed-limit [m/s] 81 | """ 82 | self._longitudinal_policies.update(speed_limit_mps) 83 | 84 | @property 85 | def num_lateral_proposals(self) -> int: 86 | return self._num_lateral_proposals 87 | 88 | @property 89 | def num_longitudinal_proposals(self) -> int: 90 | return self._longitudinal_policies._num_longitudinal_proposals 91 | 92 | @property 93 | def max_target_velocity(self) -> float: 94 | return self._longitudinal_policies.max_target_velocity 95 | 96 | @property 97 | def longitudinal_policies(self) -> BatchIDMPolicy: 98 | return self._longitudinal_policies 99 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/pdm_closed_planner.py: -------------------------------------------------------------------------------- 1 | import gc 2 | import logging 3 | import warnings 4 | from typing import List, Optional, Type 5 | 6 | from nuplan.planning.simulation.observation.observation_type import ( 7 | DetectionsTracks, 8 | Observation, 9 | ) 10 | from nuplan.planning.simulation.planner.abstract_planner import ( 11 | PlannerInitialization, 12 | PlannerInput, 13 | ) 14 | from nuplan.planning.simulation.trajectory.abstract_trajectory import AbstractTrajectory 15 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 16 | 17 | from navsim.planning.simulation.planner.pdm_planner.abstract_pdm_closed_planner import ( 18 | AbstractPDMClosedPlanner, 19 | ) 20 | from navsim.planning.simulation.planner.pdm_planner.proposal.batch_idm_policy import ( 21 | BatchIDMPolicy, 22 | ) 23 | from navsim.planning.simulation.planner.pdm_planner.observation.pdm_occupancy_map import ( 24 | PDMDrivableMap, 25 | ) 26 | 27 | warnings.filterwarnings("ignore", category=RuntimeWarning) 28 | 29 | logger = logging.getLogger(__name__) 30 | 31 | 32 | class PDMClosedPlanner(AbstractPDMClosedPlanner): 33 | """PDM-Closed planner class.""" 34 | 35 | # Inherited property, see superclass. 36 | requires_scenario: bool = False 37 | 38 | def __init__( 39 | self, 40 | trajectory_sampling: TrajectorySampling, 41 | proposal_sampling: TrajectorySampling, 42 | idm_policies: BatchIDMPolicy, 43 | lateral_offsets: Optional[List[float]], 44 | map_radius: float, 45 | ): 46 | """ 47 | Constructor for PDMClosedPlanner 48 | :param trajectory_sampling: Sampling parameters for final trajectory 49 | :param proposal_sampling: Sampling parameters for proposals 50 | :param idm_policies: BatchIDMPolicy class 51 | :param lateral_offsets: centerline offsets for proposals (optional) 52 | :param map_radius: radius around ego to consider 53 | """ 54 | super(PDMClosedPlanner, self).__init__( 55 | trajectory_sampling, 56 | proposal_sampling, 57 | idm_policies, 58 | lateral_offsets, 59 | map_radius, 60 | ) 61 | 62 | def initialize(self, initialization: PlannerInitialization) -> None: 63 | """Inherited, see superclass.""" 64 | self._iteration = 0 65 | self._map_api = initialization.map_api 66 | self._load_route_dicts(initialization.route_roadblock_ids) 67 | gc.collect() 68 | 69 | def name(self) -> str: 70 | """Inherited, see superclass.""" 71 | return self.__class__.__name__ 72 | 73 | def observation_type(self) -> Type[Observation]: 74 | """Inherited, see superclass.""" 75 | return DetectionsTracks # type: ignore 76 | 77 | def compute_planner_trajectory(self, current_input: PlannerInput) -> AbstractTrajectory: 78 | """Inherited, see superclass.""" 79 | 80 | gc.disable() 81 | ego_state, _ = current_input.history.current_state 82 | 83 | # Apply route correction on first iteration (ego_state required) 84 | if self._iteration == 0: 85 | self._route_roadblock_correction(ego_state) 86 | 87 | # Update/Create drivable area polygon map 88 | self._drivable_area_map = PDMDrivableMap.from_simulation( 89 | self._map_api, ego_state, self._map_radius 90 | ) 91 | 92 | trajectory = self._get_closed_loop_trajectory(current_input) 93 | 94 | self._iteration += 1 95 | return trajectory 96 | -------------------------------------------------------------------------------- /navsim/planning/script/run_pdm_score_from_submission.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | from tqdm import tqdm 3 | import traceback 4 | import pickle 5 | 6 | import hydra 7 | from hydra.utils import instantiate 8 | from omegaconf import DictConfig 9 | 10 | from pathlib import Path 11 | from typing import Any, Dict, List 12 | from dataclasses import asdict 13 | import logging 14 | 15 | from nuplan.planning.script.builders.logging_builder import build_logger 16 | 17 | from navsim.common.dataloader import MetricCacheLoader 18 | from navsim.evaluate.pdm_score import pdm_score 19 | from navsim.planning.simulation.planner.pdm_planner.simulation.pdm_simulator import ( 20 | PDMSimulator 21 | ) 22 | from navsim.planning.simulation.planner.pdm_planner.scoring.pdm_scorer import PDMScorer 23 | from navsim.common.dataclasses import Trajectory 24 | 25 | logger = logging.getLogger(__name__) 26 | 27 | CONFIG_PATH = "config/pdm_scoring" 28 | CONFIG_NAME = "default_run_pdm_score_from_submission" 29 | 30 | @hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME) 31 | def main(cfg: DictConfig) -> None: 32 | submission_file_path = Path(cfg.submission_file_path) 33 | metric_cache_path = Path(cfg.metric_cache_path) 34 | simulator: PDMSimulator = instantiate(cfg.simulator) 35 | scorer: PDMScorer = instantiate(cfg.scorer) 36 | build_logger(cfg) 37 | assert simulator.proposal_sampling == scorer.proposal_sampling, "Simulator and scorer proposal sampling has to be identical" 38 | 39 | run_pdm_score( 40 | submission_file_path=submission_file_path, 41 | simulator=simulator, 42 | scorer=scorer, 43 | metric_cache_path=metric_cache_path, 44 | ) 45 | 46 | def run_pdm_score( 47 | submission_file_path: Path, 48 | simulator: PDMSimulator, 49 | scorer: PDMScorer, 50 | metric_cache_path: Path, 51 | ) -> None: 52 | """ 53 | Function to evaluate an agent with the PDM-Score 54 | :param agent: Agent object 55 | :param data_path: pathlib path to navsim logs 56 | :param metric_cache_path: pathlib path to metric cache 57 | :param save_path: pathlib path to folder where scores are stored as .csv 58 | """ 59 | logger.info("Building SceneLoader") 60 | metric_cache_loader = MetricCacheLoader(metric_cache_path) 61 | with open(submission_file_path, "rb") as f: 62 | agent_output: Dict[str, Trajectory] = pickle.load(f)["predictions"] 63 | 64 | score_rows: List[Dict[str, Any]] = [] 65 | for token in tqdm(metric_cache_loader.tokens, desc="Compute PDM-Score"): 66 | score_row: Dict[str, Any] = {"token": token, "valid": True} 67 | 68 | try: 69 | metric_cache = metric_cache_loader.get_from_token(token) 70 | trajectory = agent_output[token] 71 | pdm_result = pdm_score( 72 | metric_cache=metric_cache, 73 | model_trajectory=trajectory, 74 | future_sampling=simulator.proposal_sampling, 75 | simulator=simulator, 76 | scorer=scorer, 77 | ) 78 | score_row.update(asdict(pdm_result)) 79 | except Exception as e: 80 | logger.warning(f"----------- Agent failed for token {token}:") 81 | traceback.print_exc() 82 | score_row["valid"] = False 83 | 84 | score_rows.append(score_row) 85 | 86 | pdm_score_df = pd.DataFrame(score_rows) 87 | if not pdm_score_df["valid"].all(): 88 | logger.warning("Evaluation for some tokens failed. Check log for details") 89 | else: 90 | average_score = pdm_score_df["score"].mean() 91 | return average_score 92 | 93 | if __name__ == "__main__": 94 | main() 95 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/simulation/pdm_simulator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.typing as npt 3 | from nuplan.common.actor_state.ego_state import EgoState 4 | from nuplan.common.actor_state.state_representation import TimeDuration, TimePoint 5 | from nuplan.planning.simulation.simulation_time_controller.simulation_iteration import ( 6 | SimulationIteration, 7 | ) 8 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 9 | 10 | from navsim.planning.simulation.planner.pdm_planner.simulation.batch_kinematic_bicycle import ( 11 | BatchKinematicBicycleModel, 12 | ) 13 | from navsim.planning.simulation.planner.pdm_planner.simulation.batch_lqr import ( 14 | BatchLQRTracker, 15 | ) 16 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_array_representation import ( 17 | ego_state_to_state_array, 18 | ) 19 | 20 | 21 | class PDMSimulator: 22 | """ 23 | Re-implementation of nuPlan's simulation pipeline. Enables batch-wise simulation. 24 | """ 25 | 26 | def __init__(self, proposal_sampling: TrajectorySampling): 27 | """ 28 | Constructor of PDMSimulator. 29 | :param proposal_sampling: Sampling parameters for proposals 30 | """ 31 | 32 | # time parameters 33 | self.proposal_sampling = proposal_sampling 34 | 35 | # simulation objects 36 | self._motion_model = BatchKinematicBicycleModel() 37 | self._tracker = BatchLQRTracker() 38 | 39 | def simulate_proposals( 40 | self, states: npt.NDArray[np.float64], initial_ego_state: EgoState 41 | ) -> npt.NDArray[np.float64]: 42 | """ 43 | Simulate all proposals over batch-dim 44 | :param initial_ego_state: ego-vehicle state at current iteration 45 | :param states: proposal states as array 46 | :return: simulated proposal states as array 47 | """ 48 | 49 | # TODO: find cleaner way to load parameters 50 | # set parameters of motion model and tracker 51 | self._motion_model._vehicle = initial_ego_state.car_footprint.vehicle_parameters 52 | self._tracker._discretization_time = self.proposal_sampling.interval_length 53 | 54 | proposal_states = states[:, : self.proposal_sampling.num_poses + 1] 55 | self._tracker.update(proposal_states) 56 | 57 | # state array representation for simulated vehicle states 58 | simulated_states = np.zeros(proposal_states.shape, dtype=np.float64) 59 | simulated_states[:, 0] = ego_state_to_state_array(initial_ego_state) 60 | 61 | # timing objects 62 | current_time_point = initial_ego_state.time_point 63 | delta_time_point = TimeDuration.from_s(self.proposal_sampling.interval_length) 64 | 65 | current_iteration = SimulationIteration(current_time_point, 0) 66 | next_iteration = SimulationIteration(current_time_point + delta_time_point, 1) 67 | 68 | for time_idx in range(1, self.proposal_sampling.num_poses + 1): 69 | sampling_time: TimePoint = ( 70 | next_iteration.time_point - current_iteration.time_point 71 | ) 72 | 73 | command_states = self._tracker.track_trajectory( 74 | current_iteration, 75 | next_iteration, 76 | simulated_states[:, time_idx - 1], 77 | ) 78 | 79 | simulated_states[:, time_idx] = self._motion_model.propagate_state( 80 | states=simulated_states[:, time_idx - 1], 81 | command_states=command_states, 82 | sampling_time=sampling_time, 83 | ) 84 | 85 | current_iteration = next_iteration 86 | next_iteration = SimulationIteration( 87 | current_iteration.time_point + delta_time_point, 1 + time_idx 88 | ) 89 | 90 | return simulated_states 91 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/pdm_geometry_utils.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | 3 | import numpy as np 4 | import numpy.typing as npt 5 | from nuplan.common.actor_state.state_representation import StateSE2 6 | 7 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_enums import ( 8 | SE2Index, 9 | ) 10 | 11 | 12 | def normalize_angle(angle): 13 | """ 14 | Map a angle in range [-π, π] 15 | :param angle: any angle as float 16 | :return: normalized angle 17 | """ 18 | return np.arctan2(np.sin(angle), np.cos(angle)) 19 | 20 | 21 | def parallel_discrete_path( 22 | discrete_path: List[StateSE2], offset=float 23 | ) -> List[StateSE2]: 24 | """ 25 | Creates a parallel discrete path for a given offset. 26 | :param discrete_path: baseline path (x,y,θ) 27 | :param offset: parall loffset 28 | :return: parallel discrete path 29 | """ 30 | parallel_discrete_path = [] 31 | for state in discrete_path: 32 | theta = state.heading + np.pi / 2 33 | x_new = state.x + np.cos(theta) * offset 34 | y_new = state.y + np.sin(theta) * offset 35 | parallel_discrete_path.append(StateSE2(x_new, y_new, state.heading)) 36 | return parallel_discrete_path 37 | 38 | 39 | def translate_lon_and_lat( 40 | centers: npt.NDArray[np.float64], 41 | headings: npt.NDArray[np.float64], 42 | lon: float, 43 | lat: float, 44 | ) -> npt.NDArray[np.float64]: 45 | """ 46 | Translate the position component of an centers point array 47 | :param centers: array to be translated 48 | :param headings: array with heading angles 49 | :param lon: [m] distance by which a point should be translated in longitudinal direction 50 | :param lat: [m] distance by which a point should be translated in lateral direction 51 | :return array of translated coordinates 52 | """ 53 | half_pi = np.pi / 2.0 54 | translation: npt.NDArray[np.float64] = np.stack( 55 | [ 56 | (lat * np.cos(headings + half_pi)) + (lon * np.cos(headings)), 57 | (lat * np.sin(headings + half_pi)) + (lon * np.sin(headings)), 58 | ], 59 | axis=-1, 60 | ) 61 | return centers + translation 62 | 63 | 64 | def calculate_progress(path: List[StateSE2]) -> List[float]: 65 | """ 66 | Calculate the cumulative progress of a given path. 67 | :param path: a path consisting of StateSE2 as waypoints 68 | :return: a cumulative list of progress 69 | """ 70 | x_position = [point.x for point in path] 71 | y_position = [point.y for point in path] 72 | x_diff = np.diff(x_position) 73 | y_diff = np.diff(y_position) 74 | points_diff: npt.NDArray[np.float64] = np.concatenate( 75 | ([x_diff], [y_diff]), axis=0, dtype=np.float64 76 | ) 77 | progress_diff = np.append(0.0, np.linalg.norm(points_diff, axis=0)) 78 | return np.cumsum(progress_diff, dtype=np.float64) # type: ignore 79 | 80 | 81 | def convert_absolute_to_relative_se2_array( 82 | origin: StateSE2, state_se2_array: npt.NDArray[np.float64] 83 | ) -> npt.NDArray[np.float64]: 84 | """ 85 | Converts an StateSE2 array from global to relative coordinates. 86 | :param origin: origin pose of relative coords system 87 | :param state_se2_array: array of SE2 states with (x,y,θ) in last dim 88 | :return: SE2 coords array in relative coordinates 89 | """ 90 | assert len(SE2Index) == state_se2_array.shape[-1] 91 | 92 | theta = -origin.heading 93 | origin_array = np.array([[origin.x, origin.y, origin.heading]], dtype=np.float64) 94 | 95 | R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) 96 | 97 | points_rel = state_se2_array - origin_array 98 | points_rel[..., :2] = points_rel[..., :2] @ R.T 99 | points_rel[:, 2] = normalize_angle(points_rel[:, 2]) 100 | 101 | return points_rel 102 | -------------------------------------------------------------------------------- /navsim/agents/transfuser/transfuser_config.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | from typing import Any, List, Tuple, Dict 3 | 4 | from nuplan.common.maps.abstract_map import SemanticMapLayer 5 | from nuplan.common.actor_state.tracked_objects_types import TrackedObjectType 6 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 7 | 8 | 9 | @dataclass 10 | class TransfuserConfig: 11 | 12 | trajectory_sampling: TrajectorySampling = TrajectorySampling( 13 | time_horizon=4, interval_length=0.5 14 | ) 15 | 16 | image_architecture: str = "resnet34" 17 | lidar_architecture: str = "resnet34" 18 | 19 | max_height_lidar: float = 100.0 20 | pixels_per_meter: float = 4.0 21 | hist_max_per_pixel: int = 5 22 | 23 | lidar_min_x: float = -32 24 | lidar_max_x: float = 32 25 | lidar_min_y: float = -32 26 | lidar_max_y: float = 32 27 | 28 | lidar_split_height: float = 0.2 29 | use_ground_plane: bool = False 30 | 31 | # new 32 | lidar_seq_len: int = 1 33 | 34 | camera_width: int = 1024 35 | camera_height: int = 256 36 | lidar_resolution_width = 256 37 | lidar_resolution_height = 256 38 | 39 | img_vert_anchors: int = 256 // 32 40 | img_horz_anchors: int = 1024 // 32 41 | lidar_vert_anchors: int = 256 // 32 42 | lidar_horz_anchors: int = 256 // 32 43 | 44 | block_exp = 4 45 | n_layer = 2 # Number of transformer layers used in the vision backbone 46 | n_head = 4 47 | n_scale = 4 48 | embd_pdrop = 0.1 49 | resid_pdrop = 0.1 50 | attn_pdrop = 0.1 51 | # Mean of the normal distribution initialization for linear layers in the GPT 52 | gpt_linear_layer_init_mean = 0.0 53 | # Std of the normal distribution initialization for linear layers in the GPT 54 | gpt_linear_layer_init_std = 0.02 55 | # Initial weight of the layer norms in the gpt. 56 | gpt_layer_norm_init_weight = 1.0 57 | 58 | perspective_downsample_factor = 1 59 | transformer_decoder_join = True 60 | detect_boxes = True 61 | use_bev_semantic = True 62 | use_semantic = False 63 | use_depth = False 64 | add_features = True 65 | 66 | # Transformer 67 | tf_d_model: int = 256 68 | tf_d_ffn: int = 1024 69 | tf_num_layers: int = 3 70 | tf_num_head: int = 8 71 | tf_dropout: float = 0.0 72 | 73 | # detection 74 | num_bounding_boxes: int = 30 75 | 76 | # loss weights 77 | trajectory_weight: float = 10.0 78 | agent_class_weight: float = 10.0 79 | agent_box_weight: float = 1.0 80 | bev_semantic_weight: float = 10.0 81 | 82 | # BEV mapping 83 | bev_semantic_classes = { 84 | 1: ("polygon", [SemanticMapLayer.LANE, SemanticMapLayer.INTERSECTION]), # road 85 | 2: ("polygon", [SemanticMapLayer.WALKWAYS]), # walkways 86 | 3: ("linestring", [SemanticMapLayer.LANE, SemanticMapLayer.LANE_CONNECTOR]), # centerline 87 | 4: ( 88 | "box", 89 | [ 90 | TrackedObjectType.CZONE_SIGN, 91 | TrackedObjectType.BARRIER, 92 | TrackedObjectType.TRAFFIC_CONE, 93 | TrackedObjectType.GENERIC_OBJECT, 94 | ], 95 | ), # static_objects 96 | 5: ("box", [TrackedObjectType.VEHICLE]), # vehicles 97 | 6: ("box", [TrackedObjectType.PEDESTRIAN]), # pedestrians 98 | } 99 | 100 | bev_pixel_width: int = lidar_resolution_width 101 | bev_pixel_height: int = lidar_resolution_height // 2 102 | bev_pixel_size: float = 0.25 103 | 104 | num_bev_classes = 7 105 | bev_features_channels: int = 64 106 | bev_down_sample_factor: int = 4 107 | bev_upsample_factor: int = 2 108 | 109 | @property 110 | def bev_semantic_frame(self) -> Tuple[int, int]: 111 | return (self.bev_pixel_height, self.bev_pixel_width) 112 | 113 | @property 114 | def bev_radius(self) -> float: 115 | values = [self.lidar_min_x, self.lidar_max_x, self.lidar_min_y, self.lidar_max_y] 116 | return max([abs(value) for value in values]) 117 | -------------------------------------------------------------------------------- /navsim/common/enums.py: -------------------------------------------------------------------------------- 1 | from enum import IntEnum 2 | 3 | 4 | class StateSE2Index(IntEnum): 5 | 6 | _X = 0 7 | _Y = 1 8 | _HEADING = 2 9 | 10 | @classmethod 11 | def size(cls): 12 | valid_attributes = [ 13 | attribute 14 | for attribute in dir(cls) 15 | if attribute.startswith("_") 16 | and not attribute.startswith("__") 17 | and not callable(getattr(cls, attribute)) 18 | ] 19 | return len(valid_attributes) 20 | 21 | @classmethod 22 | @property 23 | def X(cls): 24 | return cls._X 25 | 26 | @classmethod 27 | @property 28 | def Y(cls): 29 | return cls._Y 30 | 31 | @classmethod 32 | @property 33 | def HEADING(cls): 34 | return cls._HEADING 35 | 36 | @classmethod 37 | @property 38 | def POINT(cls): 39 | # assumes X, Y have subsequent indices 40 | return slice(cls._X, cls._Y + 1) 41 | 42 | @classmethod 43 | @property 44 | def STATE_SE2(cls): 45 | # assumes X, Y, HEADING have subsequent indices 46 | return slice(cls._X, cls._HEADING + 1) 47 | 48 | 49 | class BoundingBoxIndex(IntEnum): 50 | 51 | _X = 0 52 | _Y = 1 53 | _Z = 2 54 | _LENGTH = 3 55 | _WIDTH = 4 56 | _HEIGHT = 5 57 | _HEADING = 6 58 | 59 | @classmethod 60 | def size(cls): 61 | valid_attributes = [ 62 | attribute 63 | for attribute in dir(cls) 64 | if attribute.startswith("_") 65 | and not attribute.startswith("__") 66 | and not callable(getattr(cls, attribute)) 67 | ] 68 | return len(valid_attributes) 69 | 70 | @classmethod 71 | @property 72 | def X(cls): 73 | return cls._X 74 | 75 | @classmethod 76 | @property 77 | def Y(cls): 78 | return cls._Y 79 | 80 | @classmethod 81 | @property 82 | def Z(cls): 83 | return cls._Z 84 | 85 | @classmethod 86 | @property 87 | def LENGTH(cls): 88 | return cls._LENGTH 89 | 90 | @classmethod 91 | @property 92 | def WIDTH(cls): 93 | return cls._WIDTH 94 | 95 | @classmethod 96 | @property 97 | def HEIGHT(cls): 98 | return cls._HEIGHT 99 | 100 | @classmethod 101 | @property 102 | def HEADING(cls): 103 | return cls._HEADING 104 | 105 | @classmethod 106 | @property 107 | def POINT2D(cls): 108 | # assumes X, Y have subsequent indices 109 | return slice(cls._X, cls._Y + 1) 110 | 111 | @classmethod 112 | @property 113 | def POSITION(cls): 114 | # assumes X, Y, Z have subsequent indices 115 | return slice(cls._X, cls._Z + 1) 116 | 117 | @classmethod 118 | @property 119 | def DIMENSION(cls): 120 | # assumes LENGTH, WIDTH, HEIGHT have subsequent indices 121 | return slice(cls._LENGTH, cls._HEIGHT + 1) 122 | 123 | 124 | class LidarIndex(IntEnum): 125 | 126 | _X = 0 127 | _Y = 1 128 | _Z = 2 129 | _INTENSITY = 3 130 | _RING = 4 131 | _ID = 5 132 | 133 | @classmethod 134 | def size(cls): 135 | valid_attributes = [ 136 | attribute 137 | for attribute in dir(cls) 138 | if attribute.startswith("_") 139 | and not attribute.startswith("__") 140 | and not callable(getattr(cls, attribute)) 141 | ] 142 | return len(valid_attributes) 143 | 144 | @classmethod 145 | @property 146 | def X(cls): 147 | return cls._X 148 | 149 | @classmethod 150 | @property 151 | def Y(cls): 152 | return cls._Y 153 | 154 | @classmethod 155 | @property 156 | def Z(cls): 157 | return cls._Z 158 | 159 | @classmethod 160 | @property 161 | def INTENSITY(cls): 162 | return cls._INTENSITY 163 | 164 | @classmethod 165 | @property 166 | def RING(cls): 167 | return cls._RING 168 | 169 | @classmethod 170 | @property 171 | def ID(cls): 172 | return cls._ID 173 | 174 | @classmethod 175 | @property 176 | def POINT2D(cls): 177 | # assumes X, Y have subsequent indices 178 | return slice(cls._X, cls._Y + 1) 179 | 180 | @classmethod 181 | @property 182 | def POSITION(cls): 183 | # assumes X, Y, Z have subsequent indices 184 | return slice(cls._X, cls._Z + 1) 185 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/pdm_enums.py: -------------------------------------------------------------------------------- 1 | from enum import IntEnum 2 | 3 | 4 | class StateIndex: 5 | """Index mapping for array representation of ego states.""" 6 | # TODO: Update, @classmethod + @property deprecates in Python 3.13 :( 7 | 8 | _X = 0 9 | _Y = 1 10 | _HEADING = 2 11 | _VELOCITY_X = 3 12 | _VELOCITY_Y = 4 13 | _ACCELERATION_X = 5 14 | _ACCELERATION_Y = 6 15 | _STEERING_ANGLE = 7 16 | _STEERING_RATE = 8 17 | _ANGULAR_VELOCITY = 9 18 | _ANGULAR_ACCELERATION = 10 19 | 20 | @classmethod 21 | def size(cls): 22 | valid_attributes = [ 23 | attribute 24 | for attribute in dir(cls) 25 | if attribute.startswith("_") 26 | and not attribute.startswith("__") 27 | and not callable(getattr(cls, attribute)) 28 | ] 29 | return len(valid_attributes) 30 | 31 | @classmethod 32 | @property 33 | def X(cls): 34 | return cls._X 35 | 36 | @classmethod 37 | @property 38 | def Y(cls): 39 | return cls._Y 40 | 41 | @classmethod 42 | @property 43 | def HEADING(cls): 44 | return cls._HEADING 45 | 46 | @classmethod 47 | @property 48 | def VELOCITY_X(cls): 49 | return cls._VELOCITY_X 50 | 51 | @classmethod 52 | @property 53 | def VELOCITY_Y(cls): 54 | return cls._VELOCITY_Y 55 | 56 | @classmethod 57 | @property 58 | def ACCELERATION_X(cls): 59 | return cls._ACCELERATION_X 60 | 61 | @classmethod 62 | @property 63 | def ACCELERATION_Y(cls): 64 | return cls._ACCELERATION_Y 65 | 66 | @classmethod 67 | @property 68 | def STEERING_ANGLE(cls): 69 | return cls._STEERING_ANGLE 70 | 71 | @classmethod 72 | @property 73 | def STEERING_RATE(cls): 74 | return cls._STEERING_RATE 75 | 76 | @classmethod 77 | @property 78 | def ANGULAR_VELOCITY(cls): 79 | return cls._ANGULAR_VELOCITY 80 | 81 | @classmethod 82 | @property 83 | def ANGULAR_ACCELERATION(cls): 84 | return cls._ANGULAR_ACCELERATION 85 | 86 | @classmethod 87 | @property 88 | def POINT(cls): 89 | # assumes X, Y have subsequent indices 90 | return slice(cls._X, cls._Y + 1) 91 | 92 | @classmethod 93 | @property 94 | def STATE_SE2(cls): 95 | # assumes X, Y, HEADING have subsequent indices 96 | return slice(cls._X, cls._HEADING + 1) 97 | 98 | @classmethod 99 | @property 100 | def VELOCITY_2D(cls): 101 | # assumes velocity X, Y have subsequent indices 102 | return slice(cls._VELOCITY_X, cls._VELOCITY_Y + 1) 103 | 104 | @classmethod 105 | @property 106 | def ACCELERATION_2D(cls): 107 | # assumes acceleration X, Y have subsequent indices 108 | return slice(cls._ACCELERATION_X, cls._ACCELERATION_Y + 1) 109 | 110 | 111 | class SE2Index(IntEnum): 112 | """Index mapping for state se2 (x,y,θ) arrays.""" 113 | 114 | X = 0 115 | Y = 1 116 | HEADING = 2 117 | 118 | 119 | class DynamicStateIndex(IntEnum): 120 | """Index mapping for dynamic car state (output of controller).""" 121 | 122 | ACCELERATION_X = 0 123 | STEERING_RATE = 1 124 | 125 | 126 | class StateIDMIndex(IntEnum): 127 | """Index mapping for IDM states.""" 128 | 129 | PROGRESS = 0 130 | VELOCITY = 1 131 | 132 | 133 | class LeadingAgentIndex(IntEnum): 134 | """Index mapping for leading agent state (for IDM policies).""" 135 | 136 | PROGRESS = 0 137 | VELOCITY = 1 138 | LENGTH_REAR = 2 139 | 140 | 141 | class BBCoordsIndex(IntEnum): 142 | """Index mapping for corners and center of bounding boxes.""" 143 | 144 | FRONT_LEFT = 0 145 | REAR_LEFT = 1 146 | REAR_RIGHT = 2 147 | FRONT_RIGHT = 3 148 | CENTER = 4 149 | 150 | 151 | class EgoAreaIndex(IntEnum): 152 | """Index mapping for area of ego agent (used in PDMScorer).""" 153 | 154 | MULTIPLE_LANES = 0 155 | NON_DRIVABLE_AREA = 1 156 | ONCOMING_TRAFFIC = 2 157 | 158 | 159 | class MultiMetricIndex(IntEnum): 160 | """Index mapping multiplicative metrics (used in PDMScorer).""" 161 | 162 | NO_COLLISION = 0 163 | DRIVABLE_AREA = 1 164 | DRIVING_DIRECTION = 2 165 | 166 | 167 | class WeightedMetricIndex(IntEnum): 168 | """Index mapping weighted metrics (used in PDMScorer).""" 169 | 170 | PROGRESS = 0 171 | TTC = 1 172 | COMFORTABLE = 2 173 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/pdm_path.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from typing import Any, List, Tuple, Type, Union 4 | 5 | import numpy as np 6 | import numpy.typing as npt 7 | from nuplan.common.actor_state.state_representation import StateSE2 8 | from scipy.interpolate import interp1d 9 | from shapely.creation import linestrings 10 | from shapely.geometry import LineString 11 | from shapely.ops import substring 12 | import warnings 13 | 14 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_array_representation import ( 15 | array_to_states_se2, 16 | states_se2_to_array, 17 | ) 18 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_enums import ( 19 | SE2Index, 20 | ) 21 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_geometry_utils import ( 22 | calculate_progress, 23 | normalize_angle, 24 | ) 25 | 26 | 27 | class PDMPath: 28 | """Class representing a path to interpolate for PDM.""" 29 | 30 | def __init__(self, discrete_path: List[StateSE2]): 31 | """ 32 | Constructor for PDMPath 33 | :param discrete_path: list of (x,y,θ) values 34 | """ 35 | 36 | # attribute 37 | self._discrete_path = discrete_path 38 | 39 | # loaded during initialization 40 | self._states_se2_array = states_se2_to_array(discrete_path) 41 | self._states_se2_array[:, SE2Index.HEADING] = np.unwrap( 42 | self._states_se2_array[:, SE2Index.HEADING], axis=0 43 | ) 44 | self._progress = calculate_progress(discrete_path) 45 | self._linestring = linestrings(self._states_se2_array[:, : SE2Index.HEADING]) 46 | self._interpolator = interp1d(self._progress, self._states_se2_array, axis=0) 47 | 48 | def __reduce__(self) -> Tuple[Type[PDMPath], Tuple[Any, ...]]: 49 | """Helper for pickling.""" 50 | return self.__class__, (self._discrete_path, ) 51 | 52 | @property 53 | def discrete_path(self): 54 | """Getter for discrete StateSE2 objects of path.""" 55 | return self._discrete_path 56 | 57 | @property 58 | def length(self): 59 | """Getter for length of path.""" 60 | return self._progress[-1] 61 | 62 | @property 63 | def linestring(self) -> LineString: 64 | """Getter for shapely's linestring of path.""" 65 | return self._linestring 66 | 67 | def project(self, points: Any) -> Any: 68 | warnings.filterwarnings( 69 | "ignore", 70 | message="invalid value encountered in line_locate_point", 71 | category=RuntimeWarning 72 | ) 73 | return self._linestring.project(points) 74 | 75 | def interpolate( 76 | self, 77 | distances: Union[List[float], npt.NDArray[np.float64]], 78 | as_array=False, 79 | ) -> Union[npt.NDArray[np.object_], npt.NDArray[np.float64]]: 80 | """ 81 | Calculates (x,y,θ) for a given distance along the path. 82 | :param distances: list of array of distance values 83 | :param as_array: whether to return in array representation, defaults to False 84 | :return: array of StateSE2 class or (x,y,θ) values 85 | """ 86 | clipped_distances = np.clip(distances, 1e-5, self.length) 87 | interpolated_se2_array = self._interpolator(clipped_distances) 88 | interpolated_se2_array[..., 2] = normalize_angle(interpolated_se2_array[..., 2]) 89 | interpolated_se2_array[np.isnan(interpolated_se2_array)] = 0.0 90 | 91 | if as_array: 92 | return interpolated_se2_array 93 | 94 | return array_to_states_se2(interpolated_se2_array) 95 | 96 | def substring(self, start_distance: float, end_distance: float) -> LineString: 97 | """ 98 | Creates a sub-linestring between start and ending distances. 99 | :param start_distance: distance along the path to start [m] 100 | :param end_distance: distance along the path to end [m] 101 | :return: LineString 102 | """ 103 | 104 | # try faster method fist 105 | start_distance = np.clip(start_distance, 0.0, self.length) 106 | end_distance = np.clip(end_distance, 0.0, self.length) 107 | in_interval = np.logical_and( 108 | start_distance <= self._progress, self._progress <= end_distance 109 | ) 110 | coordinates = self._states_se2_array[in_interval, :2] 111 | if len(coordinates) > 1: 112 | return LineString(coordinates) 113 | 114 | # fallback to slower method of shapely 115 | return substring(self.linestring, start_distance, end_distance) 116 | -------------------------------------------------------------------------------- /navsim/agents/ego_status_mlp_agent.py: -------------------------------------------------------------------------------- 1 | from typing import Any, List, Dict 2 | from torch.optim import Optimizer 3 | from torch.optim.lr_scheduler import LRScheduler 4 | 5 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 6 | 7 | from navsim.agents.abstract_agent import AbstractAgent 8 | from navsim.common.dataclasses import AgentInput, SensorConfig 9 | from navsim.planning.training.abstract_feature_target_builder import ( 10 | AbstractFeatureBuilder, 11 | AbstractTargetBuilder, 12 | ) 13 | from navsim.common.dataclasses import Scene 14 | from typing import Union, Dict 15 | 16 | import torch 17 | 18 | 19 | class EgoStatusFeatureBuilder(AbstractFeatureBuilder): 20 | def __init__(self): 21 | pass 22 | 23 | def get_unique_name(self) -> str: 24 | return "ego_status_feature" 25 | 26 | def compute_features(self, agent_input: AgentInput) -> Dict[str, torch.Tensor]: 27 | ego_status = agent_input.ego_statuses[-1] 28 | velocity = torch.tensor(ego_status.ego_velocity) 29 | acceleration = torch.tensor(ego_status.ego_acceleration) 30 | driving_command = torch.tensor(ego_status.driving_command) 31 | ego_status_feature = torch.cat([velocity, acceleration, driving_command], dim=-1) 32 | 33 | return {"ego_status": ego_status_feature} 34 | 35 | 36 | class TrajectoryTargetBuilder(AbstractTargetBuilder): 37 | def __init__(self, trajectory_sampling: TrajectorySampling): 38 | self._trajectory_sampling = trajectory_sampling 39 | 40 | def get_unique_name(self) -> str: 41 | return "trajectory_target" 42 | 43 | def compute_targets(self, scene: Scene) -> Dict[str, torch.Tensor]: 44 | future_trajectory = scene.get_future_trajectory( 45 | num_trajectory_frames=self._trajectory_sampling.num_poses 46 | ) 47 | return {"trajectory": torch.tensor(future_trajectory.poses)} 48 | 49 | 50 | class EgoStatusMLPAgent(AbstractAgent): 51 | def __init__( 52 | self, 53 | trajectory_sampling: TrajectorySampling, 54 | hidden_layer_dim: int, 55 | lr: float, 56 | checkpoint_path: str = None, 57 | ): 58 | super().__init__() 59 | self._trajectory_sampling = trajectory_sampling 60 | self._checkpoint_path = checkpoint_path 61 | 62 | self._lr = lr 63 | 64 | self._mlp = torch.nn.Sequential( 65 | torch.nn.Linear(8, hidden_layer_dim), 66 | torch.nn.ReLU(), 67 | torch.nn.Linear(hidden_layer_dim, hidden_layer_dim), 68 | torch.nn.ReLU(), 69 | torch.nn.Linear(hidden_layer_dim, hidden_layer_dim), 70 | torch.nn.ReLU(), 71 | torch.nn.Linear(hidden_layer_dim, self._trajectory_sampling.num_poses * 3), 72 | ) 73 | 74 | def name(self) -> str: 75 | """Inherited, see superclass.""" 76 | 77 | return self.__class__.__name__ 78 | 79 | def initialize(self) -> None: 80 | """Inherited, see superclass.""" 81 | if torch.cuda.is_available(): 82 | state_dict: Dict[str, Any] = torch.load(self._checkpoint_path)["state_dict"] 83 | else: 84 | state_dict: Dict[str, Any] = torch.load( 85 | self._checkpoint_path, map_location=torch.device("cpu") 86 | )["state_dict"] 87 | self.load_state_dict({k.replace("agent.", ""): v for k, v in state_dict.items()}) 88 | 89 | def get_sensor_config(self) -> SensorConfig: 90 | """Inherited, see superclass.""" 91 | return SensorConfig.build_no_sensors() 92 | 93 | def get_target_builders(self) -> List[AbstractTargetBuilder]: 94 | return [ 95 | TrajectoryTargetBuilder(trajectory_sampling=self._trajectory_sampling), 96 | ] 97 | 98 | def get_feature_builders(self) -> List[AbstractFeatureBuilder]: 99 | return [EgoStatusFeatureBuilder()] 100 | 101 | def forward(self, features: Dict[str, torch.Tensor]) -> Dict[str, torch.Tensor]: 102 | poses: torch.Tensor = self._mlp(features["ego_status"]) 103 | return {"trajectory": poses.reshape(-1, self._trajectory_sampling.num_poses, 3)} 104 | 105 | def compute_loss( 106 | self, 107 | features: Dict[str, torch.Tensor], 108 | targets: Dict[str, torch.Tensor], 109 | predictions: Dict[str, torch.Tensor], 110 | ) -> torch.Tensor: 111 | return torch.nn.functional.l1_loss(predictions["trajectory"], targets["trajectory"]) 112 | 113 | def get_optimizers(self) -> Union[Optimizer, Dict[str, Union[Optimizer, LRScheduler]]]: 114 | return torch.optim.Adam(self._mlp.parameters(), lr=self._lr) 115 | -------------------------------------------------------------------------------- /navsim/agents/SeerDrive/modules/blocks.py: -------------------------------------------------------------------------------- 1 | from typing import List, Optional, Tuple 2 | import math 3 | import numpy as np 4 | import torch 5 | import torch.nn as nn 6 | from torch.cuda.amp.autocast_mode import autocast 7 | 8 | 9 | def linear_relu_ln(embed_dims, in_loops, out_loops, input_dims=None): 10 | if input_dims is None: 11 | input_dims = embed_dims 12 | layers = [] 13 | for _ in range(out_loops): 14 | for _ in range(in_loops): 15 | layers.append(nn.Linear(input_dims, embed_dims)) 16 | layers.append(nn.ReLU(inplace=True)) 17 | input_dims = embed_dims 18 | layers.append(nn.LayerNorm(embed_dims)) 19 | return layers 20 | 21 | 22 | def gen_sineembed_for_position(pos_tensor, hidden_dim=256): 23 | """Mostly copy-paste from https://github.com/IDEA-opensource/DAB-DETR/ 24 | """ 25 | half_hidden_dim = hidden_dim // 2 26 | scale = 2 * math.pi 27 | dim_t = torch.arange(half_hidden_dim, dtype=torch.float32, device=pos_tensor.device) 28 | dim_t = 10000 ** (2 * (dim_t // 2) / half_hidden_dim) 29 | x_embed = pos_tensor[..., 0] * scale 30 | y_embed = pos_tensor[..., 1] * scale 31 | pos_x = x_embed[..., None] / dim_t 32 | pos_y = y_embed[..., None] / dim_t 33 | pos_x = torch.stack((pos_x[..., 0::2].sin(), pos_x[..., 1::2].cos()), dim=-1).flatten(-2) 34 | pos_y = torch.stack((pos_y[..., 0::2].sin(), pos_y[..., 1::2].cos()), dim=-1).flatten(-2) 35 | pos = torch.cat((pos_y, pos_x), dim=-1) 36 | return pos 37 | 38 | 39 | def bias_init_with_prob(prior_prob): 40 | """initialize conv/fc bias value according to giving probablity.""" 41 | bias_init = float(-np.log((1 - prior_prob) / prior_prob)) 42 | return bias_init 43 | 44 | 45 | class GridSampleCrossBEVAttention(nn.Module): 46 | def __init__(self, embed_dims, num_heads, num_levels=1, in_bev_dims=64, num_points=8, config=None): 47 | super(GridSampleCrossBEVAttention, self).__init__() 48 | self.embed_dims = embed_dims 49 | self.num_heads = num_heads 50 | self.num_levels = num_levels 51 | self.num_points = num_points 52 | self.config = config 53 | self.attention_weights = nn.Linear(embed_dims,num_points) 54 | self.output_proj = nn.Linear(embed_dims, embed_dims) 55 | self.dropout = nn.Dropout(0.1) 56 | 57 | 58 | self.value_proj = nn.Sequential( 59 | nn.Conv2d(in_bev_dims, 256, kernel_size=(3, 3), stride=(1, 1), padding=1,bias=True), 60 | nn.ReLU(inplace=True), 61 | ) 62 | 63 | self.init_weight() 64 | 65 | def init_weight(self): 66 | 67 | nn.init.constant_(self.attention_weights.weight, 0) 68 | nn.init.constant_(self.attention_weights.bias, 0) 69 | 70 | nn.init.xavier_uniform_(self.output_proj.weight) 71 | nn.init.constant_(self.output_proj.bias, 0) 72 | 73 | def forward(self, queries, traj_points, bev_feature, spatial_shape): 74 | """ 75 | Args: 76 | queries: input features with shape of (bs, num_queries, embed_dims) 77 | traj_points: trajectory points with shape of (bs, num_queries, num_points, 2) 78 | bev_feature: bev features with shape of (bs, embed_dims, height, width) 79 | spatial_shapes: (height, width) 80 | 81 | """ 82 | 83 | bs, num_queries, num_points, _ = traj_points.shape 84 | 85 | # Normalize trajectory points to [-1, 1] range for grid_sample 86 | normalized_trajectory = traj_points.clone() 87 | normalized_trajectory[..., 0] = normalized_trajectory[..., 0] / self.config.lidar_max_y 88 | normalized_trajectory[..., 1] = normalized_trajectory[..., 1] / self.config.lidar_max_x 89 | 90 | normalized_trajectory = normalized_trajectory[..., [1, 0]] # Swap x and y 91 | 92 | attention_weights = self.attention_weights(queries) 93 | attention_weights = attention_weights.view(bs, num_queries, num_points).softmax(-1) 94 | 95 | value = self.value_proj(bev_feature) 96 | grid = normalized_trajectory.view(bs, num_queries, num_points, 2) 97 | # Sample features 98 | sampled_features = torch.nn.functional.grid_sample( 99 | value, 100 | grid, 101 | mode='bilinear', 102 | padding_mode='zeros', 103 | align_corners=False 104 | ) # bs, C, num_queries, num_points 105 | 106 | attention_weights = attention_weights.unsqueeze(1) 107 | out = (attention_weights * sampled_features).sum(dim=-1) 108 | out = out.permute(0, 2, 1).contiguous() # bs, num_queries, C 109 | out = self.output_proj(out) 110 | 111 | return self.dropout(out) + queries 112 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Future-Aware End-to-End Driving: Bidirectional Modeling of Trajectory Planning and Scene Evolution 2 | ### [[Paper]](https://arxiv.org/abs/2510.11092) 3 | 4 | > [**Future-Aware End-to-End Driving: Bidirectional Modeling of Trajectory Planning and Scene Evolution**](https://arxiv.org/abs/2510.11092) 5 | > [Bozhou Zhang](https://zbozhou.github.io/), [Nan Song](https://scholar.google.com/citations?hl=zh-CN&user=wLZVtjEAAAAJ), [Jingyu Li](https://github.com/Whale-ice), [Xiatian Zhu](https://scholar.google.com/citations?user=ZbA-z1cAAAAJ&hl=en), [Jiankang Deng](https://scholar.google.com/citations?user=Z_UoQFsAAAAJ&hl=zh-CN), [Li Zhang](https://lzrobots.github.io) 6 | > **NeurIPS 2025** 7 | 8 | ## Abstract 9 | End-to-end autonomous driving methods aim to directly map raw sensor inputs to future driving actions such as planned trajectories, bypassing traditional modular pipelines. While these approaches have shown promise, they often operate under a one-shot paradigm that relies heavily on the current scene context, potentially underestimating the importance of scene dynamics and their temporal evolution. This limitation restricts the model’s ability to make informed and adaptive decisions in complex driving scenarios. We propose a new perspective: the future trajectory of an autonomous vehicle is closely intertwined with the evolving dynamics of its environment, and conversely, the vehicle’s own future states can influence how the surrounding scene unfolds. Motivated by this bidirectional relationship, we introduce **SeerDrive**, a novel end-to-end framework that jointly models future scene evolution and trajectory planning in a closed-loop manner. Our method first predicts future bird’s-eye view (BEV) representations to anticipate the dynamics of the surrounding scene, then leverages this foresight to generate future-context-aware trajectories. Two key components enable this: (1) future-aware planning, which injects predicted BEV features into the trajectory planner, and (2) iterative scene modeling and vehicle planning, which refines both future scene prediction and trajectory generation through collaborative optimization. Extensive experiments on the NAVSIM and nuScenes benchmarks show that SeerDrive significantly outperforms existing state-of-the-art methods. 10 | 11 | ## News 12 | - 2025-10, we release SeerDrive. We update the code by integrating the online trajectory evaluation and selection process proposed in [WoTE](https://github.com/liyingyanUCAS/WoTE), achieving a PDMS of 88.88 without the iterative interaction between the planning and scene modeling processes. 13 | 14 | ## Pipeline 15 |
16 | 17 |

18 | 19 | ## Install the environment 20 | The environment is installed following [DiffusionDrive](https://github.com/hustvl/DiffusionDrive), and a reference procedure is provided here. 21 | ``` 22 | git clone git@github.com:LogosRoboticsGroup/SeerDrive.git 23 | 24 | conda env create -f environment.yml 25 | conda activate navsim_seerdrive 26 | 27 | git clone git@github.com:motional/nuplan-devkit.git 28 | cd nuplan-devkit/ 29 | pip install -e . 30 | 31 | cd SeerDrive/ 32 | pip install -e . 33 | 34 | pip install diffusers einops 35 | pip install rich==14.0.0 36 | ``` 37 | 38 | ## Prepare the data 39 | 1. Please follow the official [NAVSIM](https://github.com/autonomousvision/navsim) repository to prepare the dataset. 40 | 2. Please follow [WoTE](https://github.com/liyingyanUCAS/WoTE) or use the commands below to preprocess the dataset. 41 | ``` 42 | python scripts/misc/k_means_trajs.py 43 | bash scripts/misc/gen_pdm_score.sh 44 | bash scripts/evaluation/run_metric_caching.sh 45 | ``` 46 | 47 | ## Training and testing 48 | ``` 49 | # train 50 | bash scripts/training/seerdrive_train.sh 51 | 52 | # testing 53 | bash scripts/training/seerdrive_eval.sh 54 | ``` 55 | 56 | ## Results and checkpoints 57 | | NC | DAC | EP | TTC | Comfort | PDMS | Checkpoint | 58 | |:----:|:----:|:----:|:----:|:-------:|:----:|:----------:| 59 | | **98.6** | **97.1** | **82.5** | **95.2** | **100.0** | **88.9** | [Download](https://drive.google.com/file/d/1CvFsVnMhJCHZ21rTFcOKkgHHrJjteXLb/view?usp=sharing) | 60 | 61 | ## Qualitative Results 62 |
63 | 64 |

65 | 66 | ## BibTeX 67 | ```bibtex 68 | @inproceedings{zhang2025seerdrive, 69 | title={Future-Aware End-to-End Driving: Bidirectional Modeling of Trajectory Planning and Scene Evolution}, 70 | author={Zhang, Bozhou and Song, Nan and Li, Jingyu and Zhu, Xiatian and Deng, Jiankang and Zhang, Li}, 71 | booktitle={NeurIPS}, 72 | year={2025}, 73 | } 74 | ``` 75 | 76 | ## Acknowledgements 77 | - [NAVSIM](https://github.com/autonomousvision/navsim) 78 | - [DiffusionDrive](https://github.com/hustvl/DiffusionDrive) 79 | - [WoTE](https://github.com/liyingyanUCAS/WoTE) 80 | - [SSR](https://github.com/PeidongLi/SSR) 81 | -------------------------------------------------------------------------------- /navsim/agents/SeerDrive/SeerDrive_features.py: -------------------------------------------------------------------------------- 1 | from typing import Any, List, Dict, Union 2 | import torch 3 | import numpy as np 4 | from torchvision import transforms 5 | 6 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 7 | from navsim.common.enums import BoundingBoxIndex, LidarIndex 8 | from navsim.agents.abstract_agent import AbstractAgent 9 | from navsim.common.dataclasses import AgentInput, SensorConfig 10 | from navsim.planning.training.abstract_feature_target_builder import ( 11 | AbstractFeatureBuilder, 12 | AbstractTargetBuilder, 13 | ) 14 | from navsim.common.dataclasses import Scene 15 | import timm, cv2 16 | 17 | 18 | class SeerDriveFeatureBuilder(AbstractFeatureBuilder): 19 | def __init__( 20 | self, 21 | slice_indices=[3], 22 | config=None, 23 | ): 24 | self.slice_indices = slice_indices 25 | self._config = config 26 | 27 | def get_unique_name(self) -> str: 28 | return "SeerDrive_feature" 29 | 30 | def compute_features(self, agent_input: AgentInput) -> Dict[str, torch.Tensor]: 31 | """Inherited, see superclass.""" 32 | features = {} 33 | 34 | features["camera_feature"] = self._get_camera_feature(agent_input) 35 | features["lidar_feature"] = self._get_lidar_feature(agent_input) # torch.Size([1, 256, 256]) 36 | features["status_feature"] = torch.concatenate( 37 | [ 38 | torch.tensor(agent_input.ego_statuses[-1].driving_command, dtype=torch.float32), 39 | torch.tensor(agent_input.ego_statuses[-1].ego_velocity, dtype=torch.float32), 40 | torch.tensor(agent_input.ego_statuses[-1].ego_acceleration, dtype=torch.float32), 41 | ], 42 | ) # torch.Size([8]) 43 | 44 | return features 45 | 46 | def _get_camera_feature(self, agent_input: AgentInput) -> torch.Tensor: 47 | """ 48 | Extract stitched camera from AgentInput 49 | :param agent_input: input dataclass 50 | :return: stitched front view image as torch tensor 51 | """ 52 | 53 | cameras = agent_input.cameras[-1] 54 | 55 | # Crop to ensure 4:1 aspect ratio (1024, 1088, 3) 56 | l0 = cameras.cam_l0.image[28:-28, 416:-416] 57 | f0 = cameras.cam_f0.image[28:-28] 58 | r0 = cameras.cam_r0.image[28:-28, 416:-416] 59 | 60 | # stitch l0, f0, r0 images 61 | stitched_image = np.concatenate([l0, f0, r0], axis=1) # (1024, 4096, 3) 62 | resized_image = cv2.resize(stitched_image, (1024, 256)) # (256, 1024, 3) 63 | tensor_image = transforms.ToTensor()(resized_image) # torch.Size([3, 256, 1024]) 64 | 65 | return tensor_image 66 | 67 | def _get_lidar_feature(self, agent_input: AgentInput) -> torch.Tensor: 68 | """ 69 | Compute LiDAR feature as 2D histogram, according to Transfuser 70 | :param agent_input: input dataclass 71 | :return: LiDAR histogram as torch tensors 72 | """ 73 | 74 | # only consider (x,y,z) & swap axes for (N,3) numpy array 75 | lidar_pc = agent_input.lidars[-1].lidar_pc[LidarIndex.POSITION].T 76 | 77 | # NOTE: Code from 78 | # https://github.com/autonomousvision/carla_garage/blob/main/team_code/data.py#L873 79 | def splat_points(point_cloud): 80 | # 256 x 256 grid 81 | xbins = np.linspace( 82 | self._config.lidar_min_x, 83 | self._config.lidar_max_x, 84 | (self._config.lidar_max_x - self._config.lidar_min_x) 85 | * int(self._config.pixels_per_meter) 86 | + 1, 87 | ) 88 | ybins = np.linspace( 89 | self._config.lidar_min_y, 90 | self._config.lidar_max_y, 91 | (self._config.lidar_max_y - self._config.lidar_min_y) 92 | * int(self._config.pixels_per_meter) 93 | + 1, 94 | ) 95 | hist = np.histogramdd(point_cloud[:, :2], bins=(xbins, ybins))[0] 96 | hist[hist > self._config.hist_max_per_pixel] = self._config.hist_max_per_pixel 97 | overhead_splat = hist / self._config.hist_max_per_pixel 98 | return overhead_splat 99 | 100 | # Remove points above the vehicle 101 | lidar_pc = lidar_pc[lidar_pc[..., 2] < self._config.max_height_lidar] 102 | below = lidar_pc[lidar_pc[..., 2] <= self._config.lidar_split_height] 103 | above = lidar_pc[lidar_pc[..., 2] > self._config.lidar_split_height] 104 | above_features = splat_points(above) 105 | if self._config.use_ground_plane: 106 | below_features = splat_points(below) 107 | features = np.stack([below_features, above_features], axis=-1) 108 | else: 109 | features = np.stack([above_features], axis=-1) 110 | features = np.transpose(features, (2, 0, 1)).astype(np.float32) 111 | 112 | return torch.tensor(features) 113 | -------------------------------------------------------------------------------- /navsim/agents/abstract_agent.py: -------------------------------------------------------------------------------- 1 | from abc import abstractmethod, ABC 2 | from typing import Dict, Union, List 3 | import torch 4 | import pytorch_lightning as pl 5 | 6 | from navsim.common.dataclasses import AgentInput, Trajectory, SensorConfig 7 | from navsim.planning.training.abstract_feature_target_builder import AbstractFeatureBuilder, AbstractTargetBuilder 8 | 9 | 10 | class AbstractAgent(torch.nn.Module, ABC): 11 | def __init__( 12 | self, 13 | requires_scene: bool = False, 14 | ): 15 | super().__init__() 16 | self.requires_scene = requires_scene 17 | 18 | @abstractmethod 19 | def name(self) -> str: 20 | """ 21 | :return: string describing name of this agent. 22 | """ 23 | pass 24 | 25 | @abstractmethod 26 | def get_sensor_config(self) -> SensorConfig: 27 | """ 28 | :return: Dataclass defining the sensor configuration for lidar and cameras. 29 | """ 30 | pass 31 | 32 | @abstractmethod 33 | def initialize(self) -> None: 34 | """ 35 | Initialize agent 36 | :param initialization: Initialization class. 37 | """ 38 | pass 39 | 40 | def forward(self, features: Dict[str, torch.Tensor]) -> Dict[str, torch.Tensor]: 41 | """ 42 | Forward pass of the agent. 43 | :param features: Dictionary of features. 44 | :return: Dictionary of predictions. 45 | """ 46 | raise NotImplementedError 47 | 48 | def get_feature_builders(self) -> List[AbstractFeatureBuilder]: 49 | """ 50 | :return: List of target builders. 51 | """ 52 | raise NotImplementedError("No feature builders. Agent does not support training.") 53 | 54 | def get_target_builders(self) -> List[AbstractTargetBuilder]: 55 | """ 56 | :return: List of feature builders. 57 | """ 58 | raise NotImplementedError("No target builders. Agent does not support training.") 59 | 60 | def compute_trajectory(self, agent_input: AgentInput) -> Trajectory: 61 | """ 62 | Computes the ego vehicle trajectory. 63 | :param current_input: Dataclass with agent inputs. 64 | :return: Trajectory representing the predicted ego's position in future 65 | """ 66 | self.eval() 67 | features : Dict[str, torch.Tensor] = {} 68 | # build features 69 | for builder in self.get_feature_builders(): 70 | features.update(builder.compute_features(agent_input)) 71 | 72 | # add batch dimension 73 | features = {k: v.unsqueeze(0) for k, v in features.items()} 74 | 75 | # forward pass 76 | with torch.no_grad(): 77 | predictions = self.forward(features) 78 | poses = predictions["trajectory"].squeeze(0).numpy() 79 | 80 | # extract trajectory 81 | return Trajectory(poses) 82 | 83 | def compute_trajectory_gpu(self, agent_input: AgentInput) -> Trajectory: 84 | """ 85 | Computes the ego vehicle trajectory. 86 | :param current_input: Dataclass with agent inputs. 87 | :return: Trajectory representing the predicted ego's position in future 88 | """ 89 | self.eval() 90 | features : Dict[str, torch.Tensor] = {} 91 | # build features 92 | for builder in self.get_feature_builders(): 93 | features.update(builder.compute_features(agent_input)) 94 | 95 | # add batch dimension 96 | features = {k: v.unsqueeze(0).to('cuda') for k, v in features.items()} 97 | 98 | # forward pass 99 | with torch.no_grad(): 100 | predictions = self.forward(features) 101 | poses = predictions["trajectory"].squeeze(0).cpu().numpy() 102 | 103 | # extract trajectory 104 | return Trajectory(poses) 105 | 106 | def compute_loss( 107 | self, 108 | features: Dict[str, torch.Tensor], 109 | targets: Dict[str, torch.Tensor], 110 | predictions: Dict[str, torch.Tensor], 111 | ) -> torch.Tensor: 112 | """ 113 | Computes the loss used for backpropagation based on the features, targets and model predictions. 114 | """ 115 | raise NotImplementedError("No loss. Agent does not support training.") 116 | 117 | def get_optimizers( 118 | self 119 | ) -> Union[ 120 | torch.optim.Optimizer, 121 | Dict[str, Union[ 122 | torch.optim.Optimizer, 123 | torch.optim.lr_scheduler.LRScheduler] 124 | ] 125 | ]: 126 | """ 127 | Returns the optimizers that are used by thy pytorch-lightning trainer. 128 | Has to be either a single optimizer or a dict of optimizer and lr scheduler. 129 | """ 130 | raise NotImplementedError("No optimizers. Agent does not support training.") 131 | 132 | def get_training_callbacks( 133 | self 134 | ) -> List[pl.Callback]: 135 | """ 136 | Returns a list of pytorch-lightning callbacks that are used during training. 137 | See navsim.planning.training.callbacks for examples. 138 | """ 139 | return [] -------------------------------------------------------------------------------- /scripts/miscs/k_means_trajs.py: -------------------------------------------------------------------------------- 1 | import os, json 2 | from pathlib import Path 3 | from concurrent.futures import ThreadPoolExecutor 4 | from matplotlib.cm import get_cmap 5 | 6 | import hydra 7 | from hydra.utils import instantiate 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | from sklearn.cluster import KMeans 11 | from sklearn.cluster import MiniBatchKMeans 12 | 13 | from tqdm import tqdm 14 | from navsim.common.dataloader import SceneLoader 15 | from navsim.common.dataclasses import SceneFilter, SensorConfig 16 | 17 | SPLIT = "trainval" # ["mini", "test", "trainval"] 18 | FILTER = "navtrain" # ["navtrain", "navtest", "all_scenes", ] 19 | num_poses = 8 # 0.5s * 8 = 4s 20 | 21 | # 定义 K-means 的聚类数目 22 | K = 256 23 | 24 | """ 25 | save navtrain future trajectories as numpy array 26 | """ 27 | # 初始化 hydra 配置 28 | hydra.initialize(config_path="../../navsim/planning/script/config/common/scene_filter") 29 | cfg = hydra.compose(config_name=FILTER) 30 | scene_filter: SceneFilter = instantiate(cfg) 31 | openscene_data_root = Path("/Path_To_dataset/") 32 | 33 | # 创建场景加载器 34 | scene_loader = SceneLoader( 35 | openscene_data_root / f"navsim_logs/{SPLIT}", 36 | openscene_data_root / f"sensor_blobs/{SPLIT}", 37 | scene_filter, 38 | sensor_config=SensorConfig.build_no_sensors(), 39 | # sensor_config=SensorConfig.build_all_sensors(), 40 | ) 41 | 42 | future_trajectories_list = [] # 用于记录所有 future_trajectory 43 | 44 | # 并行遍历所有 tokens 45 | def process_token(token): 46 | scene = scene_loader.get_scene_from_token(token) 47 | future_trajectory = scene.get_future_trajectory( 48 | num_trajectory_frames=num_poses, 49 | ).poses 50 | return future_trajectory 51 | 52 | print("Collecting future trajectories...") 53 | for token in tqdm(scene_loader.tokens): 54 | scene = scene_loader.get_scene_from_token(token) 55 | future_trajectory = scene.get_future_trajectory( 56 | num_trajectory_frames=num_poses, 57 | ).poses 58 | future_trajectories_list.append(future_trajectory) 59 | 60 | # save future_trajectories_list as numpy array 61 | numpy_path = f"future_trajectories_list_{SPLIT}_{FILTER}.npy" 62 | np.save(numpy_path, future_trajectories_list) 63 | 64 | # load 65 | future_trajectories_list = np.load("/Path_To_SeerDrive/ckpts/extra_data/future_trajectories_list_trainval_navtrain.npy") 66 | np.set_printoptions(suppress=True) 67 | # 将 future_trajectories_list 转换为 numpy 数组，并展平每条轨迹 68 | N = len(future_trajectories_list) 69 | future_trajectories_array = np.array(future_trajectories_list) # (N, 2), the last position 70 | flattened_trajectories = future_trajectories_array.reshape(N, -1).astype(np.float32) # (N, 24) 71 | 72 | # 使用 MiniBatchKMeans 进行聚类 73 | kmeans = MiniBatchKMeans(n_clusters=K, random_state=0, batch_size=1000) 74 | kmeans.fit(flattened_trajectories) 75 | 76 | # 获取每条轨迹的聚类标签和聚类中心 77 | labels = kmeans.labels_ # 每条轨迹对应的聚类标签 78 | trajectory_anchors = kmeans.trajectory_anchors_ # 聚类中心，形状为 (K, 24) 79 | 80 | 81 | # 将聚类中心转换回原始轨迹的形状 (8, 3) 82 | trajectory_anchors = trajectory_anchors.reshape(K, 8, 3) 83 | 84 | # save trajectory_anchors as numpy array 85 | numpy_path = f"/Path_To_SeerDrive/ckpts/extra_data/planning_vb/trajectory_anchors_{K}.npy" 86 | np.save(numpy_path, trajectory_anchors) 87 | 88 | """" 89 | Visual code 90 | """ 91 | numpy_path = f"/Path_To_SeerDrive/ckpts/extra_data/planning_vb/trajectory_anchors_{K}.npy" 92 | trajectory_anchors = np.load(numpy_path) 93 | 94 | # Visualize all cluster centers on a single plot 95 | fig, ax = plt.subplots(figsize=(15, 15)) 96 | cmap = get_cmap('hsv', K) # Use colormap to distinguish between different trajectories 97 | 98 | for i in range(K): 99 | trajectory = trajectory_anchors[i] 100 | ax.plot(trajectory[:, 0], trajectory[:, 1], marker='o', color=cmap(i), label=f'Cluster {i}', alpha=0.6, linewidth=1.5) 101 | 102 | ax.set_title('All Cluster Centers') 103 | ax.set_xlabel('X Position') 104 | ax.set_ylabel('Y Position') 105 | ax.grid(False) 106 | plt.tight_layout() 107 | plt.savefig(f'/Path_To_SeerDrive/ckpts/extra_data/planning_vb/trajectory_anchors_{K}_no_grid.png') 108 | 109 | # save trajectory_anchors as numpy array 110 | # Load cluster centers data 111 | numpy_path = f"/Path_To_SeerDrive/ckpts/extra_data/planning_vb/trajectory_anchors_{K}.npy" 112 | trajectory_anchors = np.load(numpy_path) 113 | 114 | # Create a figure for plotting 115 | fig, ax = plt.subplots(figsize=(15, 15)) 116 | 117 | highlight_idx = 57 # Choose the trajectory to highlight 118 | cmap = get_cmap('hsv', K) # Use colormap for distinguishing if needed 119 | 120 | # Convert RGB (115, 137, 177) to a normalized value in [0, 1] 121 | background_color = (115/255, 137/255, 177/255) 122 | 123 | # Plot each trajectory 124 | for i in range(K): 125 | trajectory = trajectory_anchors[i] 126 | if i == highlight_idx: 127 | ax.plot(trajectory[:, 0], trajectory[:, 1], marker='o', color='red', label=f'Highlighted Cluster {i}', alpha=0.9, linewidth=5) 128 | else: 129 | ax.plot(trajectory[:, 0], trajectory[:, 1], color=background_color, alpha=0.9, linewidth=5) 130 | 131 | # Set plot properties 132 | ax.set_title('Highlighted Cluster with Background Clusters') 133 | ax.set_xlabel('X Position') 134 | ax.set_ylabel('Y Position') 135 | ax.legend(loc='upper right') 136 | ax.grid(False) 137 | 138 | # Adjust layout and save the figure 139 | plt.tight_layout() 140 | plt.savefig(f'/Path_To_SeerDrive/ckpts/trajectory_anchors_{K}_highlighted_{highlight_idx}.png') 141 | print(f"Saved figure to /Path_To_SeerDrive/ckpts/trajectory_anchors_{K}_highlighted_{highlight_idx}.png") 142 | -------------------------------------------------------------------------------- /navsim/agents/transfuser/transfuser_loss.py: -------------------------------------------------------------------------------- 1 | from typing import Dict 2 | from scipy.optimize import linear_sum_assignment 3 | 4 | import torch 5 | import torch.nn.functional as F 6 | 7 | from navsim.agents.transfuser.transfuser_config import TransfuserConfig 8 | 9 | 10 | def transfuser_loss( 11 | targets: Dict[str, torch.Tensor], predictions: Dict[str, torch.Tensor], config: TransfuserConfig 12 | ): 13 | """ 14 | Helper function calculating complete loss of Transfuser 15 | :param targets: dictionary of name tensor pairings 16 | :param predictions: dictionary of name tensor pairings 17 | :param config: global Transfuser config 18 | :return: combined loss value 19 | """ 20 | 21 | trajectory_loss = F.l1_loss(predictions["trajectory"], targets["trajectory"]) 22 | agent_class_loss, agent_box_loss = _agent_loss(targets, predictions, config) 23 | bev_semantic_loss = F.cross_entropy( 24 | predictions["bev_semantic_map"], targets["bev_semantic_map"].long() 25 | ) 26 | loss = ( 27 | config.trajectory_weight * trajectory_loss 28 | + config.agent_class_weight * agent_class_loss 29 | + config.agent_box_weight * agent_box_loss 30 | + config.bev_semantic_weight * bev_semantic_loss 31 | ) 32 | return loss 33 | 34 | 35 | def _agent_loss( 36 | targets: Dict[str, torch.Tensor], predictions: Dict[str, torch.Tensor], config: TransfuserConfig 37 | ): 38 | """ 39 | Hungarian matching loss for agent detection 40 | :param targets: dictionary of name tensor pairings 41 | :param predictions: dictionary of name tensor pairings 42 | :param config: global Transfuser config 43 | :return: detection loss 44 | """ 45 | 46 | gt_states, gt_valid = targets["agent_states"], targets["agent_labels"] 47 | pred_states, pred_logits = predictions["agent_states"], predictions["agent_labels"] 48 | 49 | # save constants 50 | batch_dim, num_instances = pred_states.shape[:2] 51 | num_gt_instances = gt_valid.sum() 52 | num_gt_instances = num_gt_instances if num_gt_instances > 0 else num_gt_instances + 1 53 | 54 | ce_cost = _get_ce_cost(gt_valid, pred_logits) 55 | l1_cost = _get_l1_cost(gt_states, pred_states, gt_valid) 56 | 57 | cost = config.agent_class_weight * ce_cost + config.agent_box_weight * l1_cost 58 | cost = cost.cpu() 59 | 60 | indices = [linear_sum_assignment(c) for i, c in enumerate(cost)] 61 | matching = [ 62 | (torch.as_tensor(i, dtype=torch.int64), torch.as_tensor(j, dtype=torch.int64)) 63 | for i, j in indices 64 | ] 65 | idx = _get_src_permutation_idx(matching) 66 | 67 | pred_states_idx = pred_states[idx] 68 | gt_states_idx = torch.cat([t[i] for t, (_, i) in zip(gt_states, indices)], dim=0) 69 | 70 | pred_valid_idx = pred_logits[idx] 71 | gt_valid_idx = torch.cat([t[i] for t, (_, i) in zip(gt_valid, indices)], dim=0).float() 72 | 73 | l1_loss = F.l1_loss(pred_states_idx, gt_states_idx, reduction="none") 74 | l1_loss = l1_loss.sum(-1) * gt_valid_idx 75 | l1_loss = l1_loss.view(batch_dim, -1).sum() / num_gt_instances 76 | 77 | ce_loss = F.binary_cross_entropy_with_logits(pred_valid_idx, gt_valid_idx, reduction="none") 78 | ce_loss = ce_loss.view(batch_dim, -1).mean() 79 | 80 | return ce_loss, l1_loss 81 | 82 | 83 | @torch.no_grad() 84 | def _get_ce_cost(gt_valid: torch.Tensor, pred_logits: torch.Tensor) -> torch.Tensor: 85 | """ 86 | Function to calculate cross-entropy cost for cost matrix. 87 | :param gt_valid: tensor of binary ground-truth labels 88 | :param pred_logits: tensor of predicted logits of neural net 89 | :return: bce cost matrix as tensor 90 | """ 91 | 92 | # NOTE: numerically stable BCE with logits 93 | # https://github.com/pytorch/pytorch/blob/c64e006fc399d528bb812ae589789d0365f3daf4/aten/src/ATen/native/Loss.cpp#L214 94 | gt_valid_expanded = gt_valid[:, :, None].detach().float() # (b, n, 1) 95 | pred_logits_expanded = pred_logits[:, None, :].detach() # (b, 1, n) 96 | 97 | max_val = torch.relu(-pred_logits_expanded) 98 | helper_term = max_val + torch.log( 99 | torch.exp(-max_val) + torch.exp(-pred_logits_expanded - max_val) 100 | ) 101 | ce_cost = (1 - gt_valid_expanded) * pred_logits_expanded + helper_term # (b, n, n) 102 | ce_cost = ce_cost.permute(0, 2, 1) 103 | 104 | return ce_cost 105 | 106 | 107 | @torch.no_grad() 108 | def _get_l1_cost( 109 | gt_states: torch.Tensor, pred_states: torch.Tensor, gt_valid: torch.Tensor 110 | ) -> torch.Tensor: 111 | """ 112 | Function to calculate L1 cost for cost matrix. 113 | :param gt_states: tensor of ground-truth bounding boxes 114 | :param pred_states: tensor of predicted bounding boxes 115 | :param gt_valid: mask of binary ground-truth labels 116 | :return: l1 cost matrix as tensor 117 | """ 118 | 119 | gt_states_expanded = gt_states[:, :, None, :2].detach() # (b, n, 1, 2) 120 | pred_states_expanded = pred_states[:, None, :, :2].detach() # (b, 1, n, 2) 121 | l1_cost = gt_valid[..., None].float() * (gt_states_expanded - pred_states_expanded).abs().sum( 122 | dim=-1 123 | ) 124 | l1_cost = l1_cost.permute(0, 2, 1) 125 | return l1_cost 126 | 127 | 128 | def _get_src_permutation_idx(indices): 129 | """ 130 | Helper function to align indices after matching 131 | :param indices: matched indices 132 | :return: permuted indices 133 | """ 134 | # permute predictions following indices 135 | batch_idx = torch.cat([torch.full_like(src, i) for i, (src, _) in enumerate(indices)]) 136 | src_idx = torch.cat([src for (src, _) in indices]) 137 | return batch_idx, src_idx 138 | -------------------------------------------------------------------------------- /navsim/agents/SeerDrive/configs/default.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | from typing import List, Tuple, Dict 3 | 4 | from nuplan.common.maps.abstract_map import SemanticMapLayer 5 | from nuplan.common.actor_state.tracked_objects_types import TrackedObjectType 6 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 7 | 8 | 9 | @dataclass 10 | class SeerDriveConfig: 11 | 12 | trajectory_sampling: TrajectorySampling = TrajectorySampling( 13 | time_horizon=4, interval_length=0.5 14 | ) 15 | 16 | # TODO 17 | resnet34_path = '/Path_To_SeerDrive/ckpts/resnet34.pth' 18 | 19 | image_architecture: str = "resnet34" 20 | lidar_architecture: str = "resnet34" 21 | 22 | max_height_lidar: float = 100.0 23 | pixels_per_meter: float = 4.0 24 | hist_max_per_pixel: int = 5 25 | 26 | num_keyval = 64 # 8*8 27 | lidar_min_x: float = -32 28 | lidar_max_x: float = 32 29 | lidar_min_y: float = -32 30 | lidar_max_y: float = 32 31 | 32 | lidar_split_height: float = 0.2 33 | use_ground_plane: bool = False 34 | 35 | # new 36 | lidar_seq_len: int = 1 37 | 38 | camera_width: int = 1024 39 | camera_height: int = 256 40 | lidar_resolution_width = 256 41 | lidar_resolution_height = 256 42 | 43 | img_vert_anchors: int = 256 // 32 44 | img_horz_anchors: int = 1024 // 32 45 | lidar_vert_anchors: int = 256 // 32 46 | lidar_horz_anchors: int = 256 // 32 47 | 48 | block_exp = 4 49 | n_layer = 2 # Number of transformer layers used in the vision backbone 50 | n_head = 4 51 | n_scale = 4 52 | embd_pdrop = 0.1 53 | resid_pdrop = 0.1 54 | attn_pdrop = 0.1 55 | # Mean of the normal distribution initialization for linear layers in the GPT 56 | gpt_linear_layer_init_mean = 0.0 57 | # Std of the normal distribution initialization for linear layers in the GPT 58 | gpt_linear_layer_init_std = 0.02 59 | # Initial weight of the layer norms in the gpt. 60 | gpt_layer_norm_init_weight = 1.0 61 | 62 | perspective_downsample_factor = 1 63 | transformer_decoder_join = True 64 | # detect_boxes = True 65 | # use_bev_semantic = True 66 | 67 | detect_boxes = False 68 | use_bev_semantic = False 69 | 70 | use_semantic = False 71 | use_depth = False 72 | add_features = True 73 | 74 | # Transformer 75 | tf_d_model: int = 256 76 | tf_d_ffn: int = 1024 77 | tf_num_layers: int = 3 78 | tf_num_head: int = 8 79 | tf_dropout: float = 0.0 80 | 81 | # detection 82 | num_bounding_boxes: int = 30 83 | 84 | # BEV mapping 85 | bev_semantic_classes = { 86 | 1: ("polygon", [SemanticMapLayer.LANE, SemanticMapLayer.INTERSECTION]), # road 87 | 2: ("polygon", [SemanticMapLayer.WALKWAYS]), # walkways 88 | 3: ("linestring", [SemanticMapLayer.LANE, SemanticMapLayer.LANE_CONNECTOR]), # centerline 89 | 4: ( 90 | "box", 91 | [ 92 | TrackedObjectType.CZONE_SIGN, 93 | TrackedObjectType.BARRIER, 94 | TrackedObjectType.TRAFFIC_CONE, 95 | TrackedObjectType.GENERIC_OBJECT, 96 | ], 97 | ), # static_objects 98 | 5: ("box", [TrackedObjectType.VEHICLE]), # vehicles 99 | 6: ("box", [TrackedObjectType.PEDESTRIAN]), # pedestrians 100 | 7: ("ego_box", [TrackedObjectType.VEHICLE]), # ego box 101 | } 102 | use_ego_box_in_map = True 103 | ego_box_map_idx = 7 104 | 105 | bev_pixel_width: int = lidar_resolution_width 106 | bev_pixel_height: int = lidar_resolution_height // 2 107 | bev_pixel_size: float = 0.25 108 | 109 | num_bev_classes = 8 110 | bev_features_channels: int = 64 111 | bev_down_sample_factor: int = 4 112 | bev_upsample_factor: int = 2 113 | 114 | @property 115 | def bev_semantic_frame(self) -> Tuple[int, int]: 116 | return (self.bev_pixel_height, self.bev_pixel_width) 117 | 118 | @property 119 | def bev_radius(self) -> float: 120 | values = [self.lidar_min_x, self.lidar_max_x, self.lidar_min_y, self.lidar_max_y] 121 | return max([abs(value) for value in values]) 122 | 123 | # New 124 | # k-means traj 125 | num_traj_anchor: int = 256 126 | 127 | use_sim_reward: bool = True 128 | 129 | # TODO 130 | sim_reward_dict_path: str = f'/Path_To_SeerDrive/ckpts/extra_data/planning_vb/formatted_pdm_score_{num_traj_anchor}.npy' 131 | cluster_file_path = f'/Path_To_SeerDrive/ckpts/extra_data/planning_vb/trajectory_anchors_{num_traj_anchor}.npy' 132 | 133 | num_plan_queries: int = 32 134 | 135 | # map loss 136 | input_target = True 137 | use_map_loss: bool = True 138 | use_focal_loss_for_map = True 139 | bev_semantic_weight: float = 10.0 140 | future_idx = 11 141 | fut_bev_semantic_weight: float = 0.1 142 | focal_loss_alpha = 0.5 143 | focal_loss_gamma = 2.0 144 | 145 | # sampled trajs for supervision 146 | num_sampled_trajs: int = 1 147 | 148 | # recurrent 149 | num_fut_timestep = 1 150 | use_traj_offset = True 151 | 152 | # optmizer 153 | use_coslr_opt = True 154 | lr_steps = [70] # not used 155 | scheduler_type = "MultiStepLR" # not used 156 | weight_decay: float = 1e-4 157 | optimizer_type = "AdamW" 158 | cfg_lr_mult = 0.1 159 | opt_paramwise_cfg = { 160 | "name":{ 161 | "image_encoder":{ 162 | "lr_mult": cfg_lr_mult 163 | } 164 | } 165 | } 166 | max_epochs = 100 167 | 168 | # loss weight 169 | traj_offset_loss_weight = 1.0 170 | offset_im_reward_weight = 0.1 171 | im_loss_weight = 1.0 172 | metric_loss_weight = 1.0 173 | -------------------------------------------------------------------------------- /docs/splits.md: -------------------------------------------------------------------------------- 1 | # Dataset splits vs. filtered training / test splits 2 | 3 | The NAVSIM framework utilizes several dataset splits for standardized training and evaluating agents. 4 | All of them use the OpenScene dataset that is divided into the dataset splits `mini`,`trainval`,`test`,`private_test_e2e`, which can all be downloaded separately. 5 | 6 | It is possible to run trainings and evaluations directly on these sets (see `Standard` in table below). 7 | Alternatively, you can run trainings and evaluations on training and validation splits that were filtered for challenging scenarios (see `NAVSIM` in table below), which is the recommended option for producing comparable and competitive results efficiently. 8 | In contrast to the dataset splits which refer to a downloadable set of logs, the training / test splits are implemented as scene filters, which define how scenes are extracted from these logs. 9 | 10 | The NAVSIM training / test splits subsample the OpenScene dataset splits. 11 | Moreover, the NAVSIM splits include overlapping scenes, while the Standard splits are non-overlapping. 12 | Specifically, `navtrain` is based on the `trainval` data and `navtest` on the `test` data. 13 | 14 | As the `trainval` sensor data is very large, we provide a separate download link, which loads only the frames needed for `navtrain`. 15 | This eases access for users that only want to run the `navtrain` split and not the `trainval` split. If you already downloaded the full `trainval` sensor data, it is **not necessary** to download the `navtrain` frames as well. 16 | The logs are always the complete dataset split. 17 | 18 | ## Overview 19 | The Table belows offers an overview on the training and test splits supported by NAVSIM. It also shows which config parameters have to be used to set the dataset split (`split`) and training/test split (`scene-filter`). 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 102 | 103 |
NameDescriptionLogsSensorsConfig parameters
StandardtrainvalLarge split for training and validating agents with regular driving recordings. Corresponds to nuPlan and downsampled to 2HZ.14GB>2000GB 37 | split=trainval
38 | scene_filter=all_scenes 39 |
testSmall split for testing agents with regular driving recordings. Corresponds to nuPlan and downsampled to 2HZ.1GB217GB 47 | split=test
48 | scene_filter=all_scenes 49 |
miniDemo split for with regular driving recordings. Corresponds to nuPlan and downsampled to 2HZ.1GB151GB 57 | split=mini
58 | scene_filter=all_scenes 59 |
NAVSIMnavtrainStandard split for training agents in NAVSIM with non-trivial driving scenes. Sensors available separately in download_navtrain.sh.-445GB* 68 | split=trainval
69 | scene_filter=navtrain 70 |
navtestStandard split for testing agents in NAVSIM with non-trivial driving scenes. Available as a filter for test split.-- 78 | split=test
79 | scene_filter=navtest 80 |
Competitionwarmup_test_e2eWarmup test split to validate submission on hugging face. Available as a filter for mini split.-- 89 | split=mini
90 | scene_filter=warmup_test_e2e 91 |
private_test_e2ePrivate test split for the challenge leaderboard on hugging face.<1GB25GB 99 | split=private_test_e2e
100 | scene_filter=private_test_e2e 101 |
104 | 105 | (*300GB without history) 106 | 107 | ## Splits 108 | 109 | The standard splits `trainval`, `test`, and `mini` are from the OpenScene dataset. Note that the data corresponds to the nuPlan dataset with a lower frequency of 2Hz. You can download all standard splits over Hugging Face with the bash scripts in [download](../download) 110 | 111 | NAVSIM provides a subset and filter of the `trainval` split, called `navtrain`. The `navtrain` split facilitates a standardized training scheme and requires significantly less sensor data storage than `travel` (445GB vs. 2100GB). If your agents don't need historical sensor inputs, you can download `navtrain` without history, which requires 300GB of storage. Note that `navtrain` can be downloaded separately via [download_navtrain.sh](https://github.com/autonomousvision/navsim/blob/main/download/download_navtrain.sh) but still requires access to the `trainval` logs. Similarly, the `navtest` split enables a standardized set for testing agents with a provided scene filter. Both `navtrain` and `navtest` are filtered to increase interesting samples in the sets. 112 | 113 | For the challenge on Hugging Face, we provide the `warmup_test_e2e` and `private_test_e2e` for the warm-up and challenge track, respectively. Note that `private_test_e2e` requires you to download the data, while `warmup_test_e2e` is a scene filter for the `mini` split. 114 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/graph_search/bfs_roadblock.py: -------------------------------------------------------------------------------- 1 | from collections import deque 2 | from typing import Dict, List, Optional, Tuple, Union 3 | 4 | from nuplan.common.maps.abstract_map import AbstractMap 5 | from nuplan.common.maps.abstract_map_objects import RoadBlockGraphEdgeMapObject 6 | 7 | 8 | class BreadthFirstSearchRoadBlock: 9 | """ 10 | A class that performs iterative breadth first search. The class operates on the roadblock graph. 11 | """ 12 | 13 | def __init__( 14 | self, 15 | start_roadblock_id: int, 16 | map_api: Optional[AbstractMap], 17 | forward_search: str = True, 18 | ): 19 | """ 20 | Constructor of BreadthFirstSearchRoadBlock class 21 | :param start_roadblock_id: roadblock id where graph starts 22 | :param map_api: map class in nuPlan 23 | :param forward_search: whether to search in driving direction, defaults to True 24 | """ 25 | self._map_api: Optional[AbstractMap] = map_api 26 | self._queue = deque([self.id_to_roadblock(start_roadblock_id), None]) 27 | self._parent: Dict[str, Optional[RoadBlockGraphEdgeMapObject]] = dict() 28 | self._forward_search = forward_search 29 | 30 | # lazy loaded 31 | self._target_roadblock_ids: List[str] = None 32 | 33 | def search( 34 | self, target_roadblock_id: Union[str, List[str]], max_depth: int 35 | ) -> Tuple[List[RoadBlockGraphEdgeMapObject], bool]: 36 | """ 37 | Apply BFS to find route to target roadblock. 38 | :param target_roadblock_id: id of target roadblock 39 | :param max_depth: maximum search depth 40 | :return: tuple of route and whether a path was found 41 | """ 42 | 43 | if isinstance(target_roadblock_id, str): 44 | target_roadblock_id = [target_roadblock_id] 45 | self._target_roadblock_ids = target_roadblock_id 46 | 47 | start_edge = self._queue[0] 48 | 49 | # Initial search states 50 | path_found: bool = False 51 | end_edge: RoadBlockGraphEdgeMapObject = start_edge 52 | end_depth: int = 1 53 | depth: int = 1 54 | 55 | self._parent[start_edge.id + f"_{depth}"] = None 56 | 57 | while self._queue: 58 | current_edge = self._queue.popleft() 59 | 60 | # Early exit condition 61 | if self._check_end_condition(depth, max_depth): 62 | break 63 | 64 | # Depth tracking 65 | if current_edge is None: 66 | depth += 1 67 | self._queue.append(None) 68 | if self._queue[0] is None: 69 | break 70 | continue 71 | 72 | # Goal condition 73 | if self._check_goal_condition(current_edge, depth, max_depth): 74 | end_edge = current_edge 75 | end_depth = depth 76 | path_found = True 77 | break 78 | 79 | neighbors = ( 80 | current_edge.outgoing_edges 81 | if self._forward_search 82 | else current_edge.incoming_edges 83 | ) 84 | 85 | # Populate queue 86 | for next_edge in neighbors: 87 | # if next_edge.id in self._candidate_lane_edge_ids_old: 88 | self._queue.append(next_edge) 89 | self._parent[next_edge.id + f"_{depth + 1}"] = current_edge 90 | end_edge = next_edge 91 | end_depth = depth + 1 92 | 93 | return self._construct_path(end_edge, end_depth), path_found 94 | 95 | def id_to_roadblock(self, id: str) -> RoadBlockGraphEdgeMapObject: 96 | """ 97 | Retrieves roadblock from map-api based on id 98 | :param id: id of roadblock 99 | :return: roadblock class 100 | """ 101 | block = self._map_api._get_roadblock(id) 102 | block = block or self._map_api._get_roadblock_connector(id) 103 | return block 104 | 105 | @staticmethod 106 | def _check_end_condition(depth: int, max_depth: int) -> bool: 107 | """ 108 | Check if the search should end regardless if the goal condition is met. 109 | :param depth: The current depth to check. 110 | :param target_depth: The target depth to check against. 111 | :return: whether depth exceeds the target depth. 112 | """ 113 | return depth > max_depth 114 | 115 | def _check_goal_condition( 116 | self, 117 | current_edge: RoadBlockGraphEdgeMapObject, 118 | depth: int, 119 | max_depth: int, 120 | ) -> bool: 121 | """ 122 | Check if the current edge is at the target roadblock at the given depth. 123 | :param current_edge: edge to check. 124 | :param depth: current depth to check. 125 | :param max_depth: maximum depth the edge should be at. 126 | :return: True if the lane edge is contain the in the target roadblock. False, otherwise. 127 | """ 128 | return current_edge.id in self._target_roadblock_ids and depth <= max_depth 129 | 130 | def _construct_path( 131 | self, end_edge: RoadBlockGraphEdgeMapObject, depth: int 132 | ) -> List[RoadBlockGraphEdgeMapObject]: 133 | """ 134 | Constructs a path when goal was found. 135 | :param end_edge: The end edge to start back propagating back to the start edge. 136 | :param depth: The depth of the target edge. 137 | :return: The constructed path as a list of RoadBlockGraphEdgeMapObject 138 | """ 139 | path = [end_edge] 140 | path_id = [end_edge.id] 141 | 142 | while self._parent[end_edge.id + f"_{depth}"] is not None: 143 | path.append(self._parent[end_edge.id + f"_{depth}"]) 144 | path_id.append(path[-1].id) 145 | end_edge = self._parent[end_edge.id + f"_{depth}"] 146 | depth -= 1 147 | 148 | if self._forward_search: 149 | path.reverse() 150 | path_id.reverse() 151 | 152 | return (path, path_id) 153 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/pdm_emergency_brake.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | 3 | import numpy as np 4 | import numpy.typing as npt 5 | from nuplan.common.actor_state.ego_state import EgoState 6 | from nuplan.common.actor_state.state_representation import ( 7 | StateSE2, 8 | StateVector2D, 9 | TimePoint, 10 | ) 11 | from nuplan.common.geometry.convert import relative_to_absolute_poses 12 | from nuplan.planning.simulation.trajectory.interpolated_trajectory import ( 13 | InterpolatedTrajectory, 14 | ) 15 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 16 | 17 | from navsim.planning.simulation.planner.pdm_planner.scoring.pdm_scorer import ( 18 | PDMScorer, 19 | ) 20 | 21 | 22 | class PDMEmergencyBrake: 23 | """Class for emergency brake maneuver of PDM-Closed.""" 24 | 25 | def __init__( 26 | self, 27 | trajectory_sampling: TrajectorySampling, 28 | time_to_infraction_threshold: float = 2.0, 29 | max_ego_speed: float = 5.0, 30 | max_long_accel: float = 2.40, 31 | min_long_accel: float = -4.05, 32 | infraction: str = "collision", 33 | ): 34 | """ 35 | Constructor for PDMEmergencyBrake 36 | :param trajectory_sampling: Sampling parameters for final trajectory 37 | :param time_to_infraction_threshold: threshold for applying brake, defaults to 2.0 38 | :param max_ego_speed: maximum speed to apply brake, defaults to 5.0 39 | :param max_long_accel: maximum longitudinal acceleration for braking, defaults to 2.40 40 | :param min_long_accel: min longitudinal acceleration for braking, defaults to -4.05 41 | :param infraction: infraction to determine braking (collision or ttc), defaults to "collision" 42 | """ 43 | 44 | # trajectory parameters 45 | self._trajectory_sampling = trajectory_sampling 46 | 47 | # braking parameters 48 | self._max_ego_speed: float = max_ego_speed # [m/s] 49 | self._max_long_accel: float = max_long_accel # [m/s^2] 50 | self._min_long_accel: float = min_long_accel # [m/s^2] 51 | 52 | # braking condition parameters 53 | self._time_to_infraction_threshold: float = time_to_infraction_threshold 54 | self._infraction: str = infraction 55 | 56 | assert self._infraction in [ 57 | "collision", 58 | "ttc", 59 | ], f"PDMEmergencyBraking: Infraction {self._infraction} not available as brake condition!" 60 | 61 | def brake_if_emergency( 62 | self, ego_state: EgoState, scores: npt.NDArray[np.float64], scorer: PDMScorer 63 | ) -> Optional[InterpolatedTrajectory]: 64 | """ 65 | Applies emergency brake only if an infraction is expected within horizon. 66 | :param ego_state: state object of ego 67 | :param scores: array of proposal scores 68 | :param metric: scorer class of PDM 69 | :return: brake trajectory or None 70 | """ 71 | 72 | trajectory = None 73 | ego_speed: float = ego_state.dynamic_car_state.speed 74 | 75 | proposal_idx = np.argmax(scores) 76 | 77 | # retrieve time to infraction depending on brake detection mode 78 | if self._infraction == "ttc": 79 | time_to_infraction = scorer.time_to_ttc_infraction(proposal_idx) 80 | 81 | elif self._infraction == "collision": 82 | time_to_infraction = scorer.time_to_at_fault_collision(proposal_idx) 83 | 84 | # check time to infraction below threshold 85 | if ( 86 | time_to_infraction <= self._time_to_infraction_threshold 87 | and ego_speed <= self._max_ego_speed 88 | ): 89 | trajectory = self._generate_trajectory(ego_state) 90 | 91 | return trajectory 92 | 93 | def _generate_trajectory(self, ego_state: EgoState) -> InterpolatedTrajectory: 94 | """ 95 | Generates trajectory for reach zero velocity. 96 | :param ego_state: state object of ego 97 | :return: InterpolatedTrajectory for braking 98 | """ 99 | current_time_point = ego_state.time_point 100 | current_velocity = ego_state.dynamic_car_state.center_velocity_2d.x 101 | current_acceleration = ego_state.dynamic_car_state.center_acceleration_2d.x 102 | 103 | target_velocity = 0.0 104 | 105 | if current_velocity > 0.2: 106 | k_p = 10.0 107 | k_d = 0.0 108 | 109 | error = -current_velocity 110 | dt_error = -current_acceleration 111 | u_t = k_p * error + k_d * dt_error 112 | 113 | error = max(min(u_t, self._max_long_accel), self._min_long_accel) 114 | correcting_velocity = 11 / 10 * (current_velocity + error) 115 | 116 | else: 117 | k_p = 4 118 | k_d = 1 119 | 120 | error = target_velocity - current_velocity 121 | dt_error = -current_acceleration 122 | 123 | u_t = k_p * error + k_d * dt_error 124 | 125 | correcting_velocity = max( 126 | min(u_t, self._max_long_accel), self._min_long_accel 127 | ) 128 | 129 | trajectory_states = [] 130 | 131 | # Propagate planned trajectory for set number of samples 132 | for sample in range(self._trajectory_sampling.num_poses + 1): 133 | time_t = self._trajectory_sampling.interval_length * sample 134 | pose = relative_to_absolute_poses( 135 | ego_state.center, [StateSE2(correcting_velocity * time_t, 0, 0)] 136 | )[0] 137 | 138 | ego_state_ = EgoState.build_from_center( 139 | center=pose, 140 | center_velocity_2d=StateVector2D(0, 0), 141 | center_acceleration_2d=StateVector2D(0, 0), 142 | tire_steering_angle=0.0, 143 | time_point=current_time_point, 144 | vehicle_parameters=ego_state.car_footprint.vehicle_parameters, 145 | ) 146 | trajectory_states.append(ego_state_) 147 | 148 | current_time_point += TimePoint( 149 | int(self._trajectory_sampling.interval_length * 1e6) 150 | ) 151 | 152 | return InterpolatedTrajectory(trajectory_states) 153 | -------------------------------------------------------------------------------- /navsim/planning/scenario_builder/navsim_scenario_utils.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, List, Optional 2 | import numpy as np 3 | import numpy.typing as npt 4 | 5 | 6 | from nuplan.common.actor_state.tracked_objects_types import ( 7 | TrackedObjectType, 8 | AGENT_TYPES, 9 | ) 10 | 11 | from nuplan.common.actor_state.agent import Agent 12 | from nuplan.common.actor_state.static_object import StaticObject 13 | 14 | from nuplan.common.actor_state.oriented_box import OrientedBox 15 | from nuplan.common.actor_state.scene_object import SceneObjectMetadata 16 | from nuplan.common.actor_state.ego_state import EgoState 17 | from nuplan.common.actor_state.state_representation import StateSE2, StateVector2D 18 | from nuplan.common.actor_state.tracked_objects import TrackedObjects, TrackedObject 19 | from nuplan.planning.simulation.observation.observation_type import DetectionsTracks 20 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 21 | 22 | from navsim.common.dataclasses import Annotations 23 | from navsim.common.enums import BoundingBoxIndex 24 | 25 | # TODO: Refactor this file 26 | tracked_object_types: Dict[str, TrackedObjectType] = { 27 | "vehicle": TrackedObjectType.VEHICLE, 28 | "pedestrian": TrackedObjectType.PEDESTRIAN, 29 | "bicycle": TrackedObjectType.BICYCLE, 30 | "traffic_cone": TrackedObjectType.TRAFFIC_CONE, 31 | "barrier": TrackedObjectType.BARRIER, 32 | "czone_sign": TrackedObjectType.CZONE_SIGN, 33 | "generic_object": TrackedObjectType.GENERIC_OBJECT, 34 | "ego": TrackedObjectType.EGO, 35 | } 36 | 37 | 38 | def normalize_angle(angle): 39 | """ 40 | Map a angle in range [-π, π] 41 | :param angle: any angle as float 42 | :return: normalized angle 43 | """ 44 | return np.arctan2(np.sin(angle), np.cos(angle)) 45 | 46 | 47 | def annotations_to_detection_tracks(annotations: Annotations, ego_state: EgoState): 48 | 49 | detection_tracks: List[TrackedObject] = [] 50 | 51 | time_point = ego_state.time_point 52 | track_boxes = gt_boxes_oriented_box(annotations.boxes, ego_state) 53 | 54 | for track_idx, track_box in enumerate(track_boxes): 55 | track_type = tracked_object_types[annotations.names[track_idx]] 56 | track_metadata = SceneObjectMetadata( 57 | time_point.time_us, 58 | token=annotations.instance_tokens[track_idx], 59 | track_id=None, 60 | track_token=annotations.track_tokens[track_idx], 61 | ) 62 | 63 | if track_type in AGENT_TYPES: 64 | vx, vy = ( 65 | annotations.velocity_3d[track_idx][0], 66 | annotations.velocity_3d[track_idx][1], 67 | ) 68 | velocity = StateVector2D(vx, vy) 69 | 70 | detection_track = Agent( 71 | tracked_object_type=track_type, 72 | oriented_box=track_box, 73 | velocity=rotate_vector(velocity, ego_state.rear_axle.heading), 74 | metadata=track_metadata, 75 | ) 76 | else: 77 | detection_track = StaticObject( 78 | tracked_object_type=track_type, 79 | oriented_box=track_box, 80 | metadata=track_metadata, 81 | ) 82 | 83 | detection_tracks.append(detection_track) 84 | 85 | return DetectionsTracks(TrackedObjects(detection_tracks)) 86 | 87 | 88 | def gt_boxes_oriented_box( 89 | gt_boxes: List[npt.NDArray[np.float32]], ego_state: EgoState 90 | ) -> List[OrientedBox]: 91 | 92 | oriented_boxes: List[OrientedBox] = [] 93 | for gt_box in gt_boxes: 94 | # gt_box = (x, y, z, length, width, height, yaw) TODO: add intenum 95 | local_box_x, local_box_y, local_box_heading = ( 96 | gt_box[BoundingBoxIndex.X], 97 | gt_box[BoundingBoxIndex.Y], 98 | gt_box[BoundingBoxIndex.HEADING], 99 | ) 100 | local_box_se2 = rotate_state_se2( 101 | StateSE2(local_box_x, local_box_y, local_box_heading), 102 | angle=ego_state.rear_axle.heading, 103 | ) 104 | 105 | global_box_x, global_box_y, global_box_heading = ( 106 | local_box_se2.x + ego_state.rear_axle.x, 107 | local_box_se2.y + ego_state.rear_axle.y, 108 | normalize_angle(local_box_se2.heading), 109 | ) 110 | box_length, box_width, box_height = gt_box[3], gt_box[4], gt_box[5] 111 | oriented_box = OrientedBox( 112 | StateSE2(global_box_x, global_box_y, global_box_heading), 113 | box_length, 114 | box_width, 115 | box_height, 116 | ) 117 | oriented_boxes.append(oriented_box) 118 | 119 | return oriented_boxes 120 | 121 | 122 | def rotate_state_se2(state_se2: StateSE2, angle: float = np.deg2rad(0)) -> StateSE2: 123 | 124 | sin, cos = np.sin(angle), np.cos(angle) 125 | x_rotated = state_se2.x * cos - state_se2.y * sin 126 | y_rotated = state_se2.x * sin + state_se2.y * cos 127 | heading_rotated = normalize_angle(state_se2.heading + angle) 128 | 129 | return StateSE2(x_rotated, y_rotated, heading_rotated) 130 | 131 | 132 | def rotate_vector(vector: StateVector2D, angle: float) -> StateVector2D: 133 | sin, cos = np.sin(angle), np.cos(angle) 134 | x_rotated = vector.x * cos - vector.y * sin 135 | y_rotated = vector.x * sin + vector.y * cos 136 | return StateVector2D(x_rotated, y_rotated) 137 | 138 | 139 | def sample_future_indices( 140 | future_sampling: TrajectorySampling, 141 | iteration: int, 142 | time_horizon: float, 143 | num_samples: Optional[int], 144 | ) -> List[int]: 145 | time_interval = future_sampling.interval_length 146 | if time_horizon <= 0.0 or time_interval <= 0.0 or time_horizon < time_interval: 147 | raise ValueError( 148 | f"Time horizon {time_horizon} must be greater or equal than target time interval {time_interval}" 149 | " and both must be positive." 150 | ) 151 | 152 | num_samples = num_samples if num_samples else int(time_horizon / time_interval) 153 | 154 | num_intervals = int(time_horizon / time_interval) + 1 155 | step_size = num_intervals // num_samples 156 | try: 157 | time_idcs = np.arange(iteration, num_intervals, step_size) 158 | except: 159 | assert None 160 | 161 | return list(time_idcs) 162 | -------------------------------------------------------------------------------- /navsim/planning/script/builders/simulation_builder.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | from typing import List, Optional 4 | 5 | from hydra.utils import instantiate 6 | from omegaconf import DictConfig 7 | 8 | from nuplan.common.utils.distributed_scenario_filter import DistributedMode, DistributedScenarioFilter 9 | from nuplan.planning.scenario_builder.nuplan_db.nuplan_scenario_builder import NuPlanScenarioBuilder 10 | from nuplan.planning.script.builders.metric_builder import build_metrics_engines 11 | from nuplan.planning.script.builders.utils.utils_type import is_target_type 12 | from nuplan.planning.simulation.callback.abstract_callback import AbstractCallback 13 | from nuplan.planning.simulation.callback.metric_callback import MetricCallback 14 | from nuplan.planning.simulation.callback.multi_callback import MultiCallback 15 | from nuplan.planning.simulation.controller.abstract_controller import AbstractEgoController 16 | from nuplan.planning.simulation.observation.abstract_observation import AbstractObservation 17 | from nuplan.planning.simulation.planner.abstract_planner import AbstractPlanner 18 | from nuplan.planning.simulation.runner.simulations_runner import SimulationRunner 19 | from nuplan.planning.simulation.simulation import Simulation 20 | from nuplan.planning.simulation.simulation_setup import SimulationSetup 21 | from nuplan.planning.simulation.simulation_time_controller.abstract_simulation_time_controller import ( 22 | AbstractSimulationTimeController, 23 | ) 24 | from nuplan.planning.utils.multithreading.worker_pool import WorkerPool 25 | 26 | from navsim.planning.script.builders.planner_builder import build_planners 27 | from navsim.planning.script.builders.observation_builder import build_observations 28 | 29 | 30 | logger = logging.getLogger(__name__) 31 | 32 | def build_simulations( 33 | cfg: DictConfig, 34 | worker: WorkerPool, 35 | callbacks: List[AbstractCallback], 36 | callbacks_worker: Optional[WorkerPool] = None, 37 | pre_built_planners: Optional[List[AbstractPlanner]] = None, 38 | ) -> List[SimulationRunner]: 39 | """ 40 | Build simulations. 41 | :param cfg: DictConfig. Configuration that is used to run the experiment. 42 | :param callbacks: Callbacks for simulation. 43 | :param worker: Worker for job execution. 44 | :param callbacks_worker: worker pool to use for callbacks from sim 45 | :param pre_built_planners: List of pre-built planners to run in simulation. 46 | :return A dict of simulation engines with challenge names. 47 | """ 48 | logger.info('Building simulations...') 49 | 50 | # Create Simulation object container 51 | simulations = list() 52 | 53 | # Retrieve scenarios 54 | 55 | logger.info('Extracting scenarios...') 56 | 57 | # Only allow simulation with NuPlanScenarioBuilder except when the NUPLAN_SIMULATION_ALLOW_ANY_BUILDER environment variable is set to a non-zero value. 58 | if not int(os.environ.get("NUPLAN_SIMULATION_ALLOW_ANY_BUILDER", "0")) and not is_target_type( 59 | cfg.scenario_builder, NuPlanScenarioBuilder 60 | ): 61 | raise ValueError(f"Simulation framework only runs with NuPlanScenarioBuilder. Got {cfg.scenario_builder}") 62 | 63 | scenario_filter = DistributedScenarioFilter( 64 | cfg=cfg, 65 | worker=worker, 66 | node_rank=int(os.environ.get("NODE_RANK", 0)), 67 | num_nodes=int(os.environ.get("NUM_NODES", 1)), 68 | synchronization_path=cfg.output_dir, 69 | timeout_seconds=cfg.distributed_timeout_seconds, 70 | distributed_mode=DistributedMode[cfg.distributed_mode], 71 | ) 72 | scenarios = scenario_filter.get_scenarios() 73 | 74 | metric_engines_map = {} 75 | if cfg.run_metric: 76 | logger.info('Building metric engines...') 77 | metric_engines_map = build_metrics_engines(cfg=cfg, scenarios=scenarios) 78 | logger.info('Building metric engines...DONE') 79 | else: 80 | logger.info('Metric engine is disable') 81 | 82 | logger.info('Building simulations from %d scenarios...', len(scenarios)) 83 | 84 | # Build a metric metadata file 85 | for scenario in scenarios: 86 | 87 | # Build planners 88 | if pre_built_planners is None: 89 | if 'planner' not in cfg.keys(): 90 | raise KeyError('Planner not specified in config. Please specify a planner using "planner" field.') 91 | 92 | planners = build_planners(cfg.planner, scenario) 93 | else: 94 | planners = pre_built_planners 95 | 96 | for planner in planners: 97 | # Ego Controller 98 | ego_controller: AbstractEgoController = instantiate(cfg.ego_controller, scenario=scenario) 99 | 100 | # Simulation Manager 101 | simulation_time_controller: AbstractSimulationTimeController = instantiate( 102 | cfg.simulation_time_controller, scenario=scenario 103 | ) 104 | 105 | # Perception 106 | observations: AbstractObservation = build_observations(cfg.observation, scenario=scenario) 107 | 108 | # Metric Engine 109 | metric_engine = metric_engines_map.get(scenario.scenario_type, None) 110 | if metric_engine is not None: 111 | stateful_callbacks = [MetricCallback(metric_engine=metric_engine, worker_pool=callbacks_worker)] 112 | else: 113 | stateful_callbacks = [] 114 | 115 | if "simulation_log_callback" in cfg.callback: 116 | stateful_callbacks.append( 117 | instantiate(cfg.callback["simulation_log_callback"], worker_pool=callbacks_worker) 118 | ) 119 | 120 | # Construct simulation and manager 121 | simulation_setup = SimulationSetup( 122 | time_controller=simulation_time_controller, 123 | observations=observations, 124 | ego_controller=ego_controller, 125 | scenario=scenario, 126 | ) 127 | 128 | simulation = Simulation( 129 | simulation_setup=simulation_setup, 130 | callback=MultiCallback(callbacks + stateful_callbacks), 131 | simulation_history_buffer_duration=cfg.simulation_history_buffer_duration, 132 | ) 133 | simulations.append(SimulationRunner(simulation, planner)) 134 | 135 | logger.info('Building simulations...DONE!') 136 | return simulations -------------------------------------------------------------------------------- /navsim/planning/script/run_training.py: -------------------------------------------------------------------------------- 1 | from typing import Tuple 2 | import hydra 3 | from hydra.utils import instantiate 4 | import logging, torch 5 | from omegaconf import DictConfig 6 | from pathlib import Path 7 | import pytorch_lightning as pl 8 | from torch.utils.data import DataLoader 9 | import os 10 | from pytorch_lightning.callbacks import ( 11 | LearningRateMonitor, 12 | ModelCheckpoint, 13 | RichModelSummary, 14 | RichProgressBar, 15 | ) 16 | 17 | from navsim.planning.training.dataset import CacheOnlyDataset, Dataset 18 | from navsim.planning.training.agent_lightning_module import AgentLightningModule 19 | from navsim.common.dataloader import SceneLoader 20 | from navsim.common.dataclasses import SceneFilter 21 | from navsim.agents.abstract_agent import AbstractAgent 22 | 23 | logger = logging.getLogger(__name__) 24 | 25 | CONFIG_PATH = "config/training" 26 | CONFIG_NAME = "default_training" 27 | 28 | def build_datasets(cfg: DictConfig, agent: AbstractAgent) -> Tuple[Dataset, Dataset]: 29 | train_scene_filter: SceneFilter = instantiate(cfg.scene_filter) 30 | if train_scene_filter.log_names is not None: 31 | # train_scene_filter.log_names = [l for l in train_scene_filter.log_names if l in cfg.train_logs] 32 | train_scene_filter.log_names = list(set(train_scene_filter.log_names) & set(cfg.train_logs)) 33 | else: 34 | train_scene_filter.log_names = cfg.train_logs 35 | 36 | val_scene_filter: SceneFilter = instantiate(cfg.scene_filter) 37 | if val_scene_filter.log_names is not None: 38 | # val_scene_filter.log_names = [l for l in val_scene_filter.log_names if l in cfg.val_logs] 39 | val_scene_filter.log_names = list(set(val_scene_filter.log_names) & set(cfg.val_logs)) 40 | else: 41 | val_scene_filter.log_names = cfg.val_logs 42 | 43 | data_path = Path(cfg.navsim_log_path) 44 | sensor_blobs_path = Path(cfg.sensor_blobs_path) 45 | train_debug = cfg.train_debug if hasattr(cfg, "train_debug") else False 46 | 47 | train_scene_loader = SceneLoader( 48 | sensor_blobs_path=sensor_blobs_path, 49 | data_path=data_path, 50 | scene_filter=train_scene_filter, 51 | sensor_config=agent.get_sensor_config(), 52 | train_debug=train_debug, 53 | ) 54 | 55 | val_scene_loader = SceneLoader( 56 | sensor_blobs_path=sensor_blobs_path, 57 | data_path=data_path, 58 | scene_filter=val_scene_filter, 59 | sensor_config=agent.get_sensor_config(), 60 | ) 61 | 62 | use_fut_frames = agent.config.use_fut_frames if hasattr(agent.config, "use_fut_frames") else False 63 | train_data = Dataset( 64 | scene_loader=train_scene_loader, 65 | feature_builders=agent.get_feature_builders(), 66 | target_builders=agent.get_target_builders(), 67 | cache_path=cfg.cache_path, 68 | force_cache_computation=cfg.force_cache_computation, 69 | use_fut_frames=use_fut_frames, 70 | ) 71 | 72 | val_data = Dataset( 73 | scene_loader=val_scene_loader, 74 | feature_builders=agent.get_feature_builders(), 75 | target_builders=agent.get_target_builders(), 76 | cache_path=cfg.cache_path, 77 | force_cache_computation=cfg.force_cache_computation, 78 | use_fut_frames=use_fut_frames, 79 | ) 80 | 81 | return train_data, val_data 82 | 83 | 84 | @hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME) 85 | def main(cfg: DictConfig) -> None: 86 | logger.info("Global Seed set to 0") 87 | pl.seed_everything(0, workers=True) 88 | 89 | logger.info(f"Path where all results are stored: {cfg.output_dir}") 90 | 91 | logger.info("Building Agent") 92 | agent: AbstractAgent = instantiate(cfg.agent) 93 | 94 | logger.info("Building Lightning Module") 95 | lightning_module = AgentLightningModule( 96 | agent=agent, 97 | ) 98 | 99 | if cfg.use_cache_without_dataset: 100 | logger.info("Using cached data without building SceneLoader") 101 | assert cfg.force_cache_computation==False, "force_cache_computation must be False when using cached data without building SceneLoader" 102 | assert cfg.cache_path is not None, "cache_path must be provided when using cached data without building SceneLoader" 103 | train_data = CacheOnlyDataset( 104 | cache_path=cfg.cache_path, 105 | feature_builders=agent.get_feature_builders(), 106 | target_builders=agent.get_target_builders(), 107 | log_names=cfg.train_logs, 108 | ) 109 | val_data = CacheOnlyDataset( 110 | cache_path=cfg.cache_path, 111 | feature_builders=agent.get_feature_builders(), 112 | target_builders=agent.get_target_builders(), 113 | log_names=cfg.val_logs, 114 | ) 115 | else: 116 | logger.info("Building SceneLoader") 117 | train_data, val_data = build_datasets(cfg, agent) 118 | 119 | logger.info("Building Datasets") 120 | train_dataloader = DataLoader(train_data, **cfg.dataloader.params, shuffle=True) 121 | logger.info("Num training samples: %d", len(train_data)) 122 | val_dataloader = DataLoader(val_data, **cfg.dataloader.params, shuffle=False) 123 | logger.info("Num validation samples: %d", len(val_data)) 124 | 125 | logger.info("Building Trainer") 126 | trainer_params = cfg.trainer.params 127 | callbacks = [ 128 | ModelCheckpoint( 129 | dirpath=os.path.join(cfg.output_dir, "checkpoints"), 130 | filename="{epoch}_{train_total_loss:.3f}", 131 | monitor="train_total_loss", 132 | mode="min", 133 | save_top_k=10, 134 | save_last=True, 135 | save_on_train_epoch_end=True, 136 | ), 137 | RichModelSummary(max_depth=1), 138 | RichProgressBar(), 139 | LearningRateMonitor(logging_interval="epoch"), 140 | ] 141 | # trainer_params['strategy'] = "ddp_find_unused_parameters_true" 142 | trainer = pl.Trainer( 143 | **trainer_params, 144 | callbacks=callbacks, 145 | # callbacks=agent.get_training_callbacks(), 146 | ) 147 | 148 | logger.info("Starting Training") 149 | # ckpt_path = '' 150 | trainer.fit( 151 | model=lightning_module, 152 | train_dataloaders=train_dataloader, 153 | val_dataloaders=val_dataloader, 154 | # ckpt_path=ckpt_path, 155 | ) 156 | 157 | if __name__ == "__main__": 158 | main() -------------------------------------------------------------------------------- /navsim/agents/transfuser/transfuser_model.py: -------------------------------------------------------------------------------- 1 | from typing import Dict 2 | import numpy as np 3 | import torch 4 | import torch.nn as nn 5 | 6 | from navsim.agents.transfuser.transfuser_config import TransfuserConfig 7 | from navsim.agents.transfuser.transfuser_backbone import TransfuserBackbone 8 | from navsim.common.enums import StateSE2Index 9 | from navsim.agents.transfuser.transfuser_features import BoundingBox2DIndex 10 | 11 | 12 | class TransfuserModel(nn.Module): 13 | def __init__(self, config: TransfuserConfig): 14 | 15 | super().__init__() 16 | 17 | self._query_splits = [ 18 | 1, 19 | config.num_bounding_boxes, 20 | ] 21 | 22 | self._config = config 23 | self._backbone = TransfuserBackbone(config) 24 | 25 | self._keyval_embedding = nn.Embedding( 26 | 8**2 + 1, config.tf_d_model 27 | ) # 8x8 feature grid + trajectory 28 | self._query_embedding = nn.Embedding(sum(self._query_splits), config.tf_d_model) 29 | 30 | # usually, the BEV features are variable in size. 31 | self._bev_downscale = nn.Conv2d(512, config.tf_d_model, kernel_size=1) 32 | self._status_encoding = nn.Linear(4 + 2 + 2, config.tf_d_model) 33 | 34 | self._bev_semantic_head = nn.Sequential( 35 | nn.Conv2d( 36 | config.bev_features_channels, 37 | config.bev_features_channels, 38 | kernel_size=(3, 3), 39 | stride=1, 40 | padding=(1, 1), 41 | bias=True, 42 | ), 43 | nn.ReLU(inplace=True), 44 | nn.Conv2d( 45 | config.bev_features_channels, 46 | config.num_bev_classes, 47 | kernel_size=(1, 1), 48 | stride=1, 49 | padding=0, 50 | bias=True, 51 | ), 52 | nn.Upsample( 53 | size=(config.lidar_resolution_height // 2, config.lidar_resolution_width), 54 | mode="bilinear", 55 | align_corners=False, 56 | ), 57 | ) 58 | 59 | tf_decoder_layer = nn.TransformerDecoderLayer( 60 | d_model=config.tf_d_model, 61 | nhead=config.tf_num_head, 62 | dim_feedforward=config.tf_d_ffn, 63 | dropout=config.tf_dropout, 64 | batch_first=True, 65 | ) 66 | 67 | self._tf_decoder = nn.TransformerDecoder(tf_decoder_layer, config.tf_num_layers) 68 | self._agent_head = AgentHead( 69 | num_agents=config.num_bounding_boxes, 70 | d_ffn=config.tf_d_ffn, 71 | d_model=config.tf_d_model, 72 | ) 73 | 74 | self._trajectory_head = TrajectoryHead( 75 | num_poses=config.trajectory_sampling.num_poses, 76 | d_ffn=config.tf_d_ffn, 77 | d_model=config.tf_d_model, 78 | ) 79 | 80 | def forward(self, features: Dict[str, torch.Tensor]) -> Dict[str, torch.Tensor]: 81 | 82 | camera_feature: torch.Tensor = features["camera_feature"] 83 | lidar_feature: torch.Tensor = features["lidar_feature"] 84 | status_feature: torch.Tensor = features["status_feature"] 85 | 86 | batch_size = status_feature.shape[0] 87 | 88 | bev_feature_upscale, bev_feature, _ = self._backbone(camera_feature, lidar_feature) 89 | 90 | bev_feature = self._bev_downscale(bev_feature).flatten(-2, -1) 91 | bev_feature = bev_feature.permute(0, 2, 1) 92 | status_encoding = self._status_encoding(status_feature) 93 | 94 | keyval = torch.concatenate([bev_feature, status_encoding[:, None]], dim=1) 95 | keyval += self._keyval_embedding.weight[None, ...] 96 | 97 | query = self._query_embedding.weight[None, ...].repeat(batch_size, 1, 1) 98 | query_out = self._tf_decoder(query, keyval) 99 | 100 | bev_semantic_map = self._bev_semantic_head(bev_feature_upscale) 101 | trajectory_query, agents_query = query_out.split(self._query_splits, dim=1) 102 | 103 | output: Dict[str, torch.Tensor] = {"bev_semantic_map": bev_semantic_map} 104 | trajectory = self._trajectory_head(trajectory_query) 105 | output.update(trajectory) 106 | 107 | agents = self._agent_head(agents_query) 108 | output.update(agents) 109 | 110 | return output 111 | 112 | 113 | class AgentHead(nn.Module): 114 | def __init__( 115 | self, 116 | num_agents: int, 117 | d_ffn: int, 118 | d_model: int, 119 | ): 120 | super(AgentHead, self).__init__() 121 | 122 | self._num_objects = num_agents 123 | self._d_model = d_model 124 | self._d_ffn = d_ffn 125 | 126 | self._mlp_states = nn.Sequential( 127 | nn.Linear(self._d_model, self._d_ffn), 128 | nn.ReLU(), 129 | nn.Linear(self._d_ffn, BoundingBox2DIndex.size()), 130 | ) 131 | 132 | self._mlp_label = nn.Sequential( 133 | nn.Linear(self._d_model, 1), 134 | ) 135 | 136 | def forward(self, agent_queries) -> Dict[str, torch.Tensor]: 137 | 138 | agent_states = self._mlp_states(agent_queries) # agent_states: torch.Size([32, 30, 5]) 139 | agent_states[..., BoundingBox2DIndex.POINT] = ( 140 | agent_states[..., BoundingBox2DIndex.POINT].tanh() * 32 141 | ) 142 | agent_states[..., BoundingBox2DIndex.HEADING] = ( 143 | agent_states[..., BoundingBox2DIndex.HEADING].tanh() * np.pi 144 | ) 145 | 146 | agent_labels = self._mlp_label(agent_queries).squeeze(dim=-1) 147 | 148 | return {"agent_states": agent_states, "agent_labels": agent_labels} 149 | 150 | 151 | class TrajectoryHead(nn.Module): 152 | def __init__(self, num_poses: int, d_ffn: int, d_model: int): 153 | super(TrajectoryHead, self).__init__() 154 | 155 | self._num_poses = num_poses 156 | self._d_model = d_model 157 | self._d_ffn = d_ffn 158 | 159 | self._mlp = nn.Sequential( 160 | nn.Linear(self._d_model, self._d_ffn), 161 | nn.ReLU(), 162 | nn.Linear(self._d_ffn, num_poses * StateSE2Index.size()), 163 | ) 164 | 165 | def forward(self, object_queries) -> Dict[str, torch.Tensor]: 166 | poses = self._mlp(object_queries).reshape(-1, self._num_poses, StateSE2Index.size()) 167 | poses[..., StateSE2Index.HEADING] = poses[..., StateSE2Index.HEADING].tanh() * np.pi 168 | return {"trajectory": poses} 169 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/utils/graph_search/dijkstra.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, List, Optional, Tuple 2 | 3 | import numpy as np 4 | from nuplan.common.maps.abstract_map_objects import ( 5 | LaneGraphEdgeMapObject, 6 | RoadBlockGraphEdgeMapObject, 7 | ) 8 | 9 | 10 | class Dijkstra: 11 | """ 12 | A class that performs dijkstra's shortest path. The class operates on lane level graph search. 13 | The goal condition is specified to be if the lane can be found at the target roadblock or roadblock connector. 14 | """ 15 | 16 | def __init__( 17 | self, start_edge: LaneGraphEdgeMapObject, candidate_lane_edge_ids: List[str] 18 | ): 19 | """ 20 | Constructor for the Dijkstra class. 21 | :param start_edge: The starting edge for the search 22 | :param candidate_lane_edge_ids: The candidates lane ids that can be included in the search. 23 | """ 24 | self._queue = list([start_edge]) 25 | self._parent: Dict[str, Optional[LaneGraphEdgeMapObject]] = dict() 26 | self._candidate_lane_edge_ids = candidate_lane_edge_ids 27 | 28 | def search( 29 | self, target_roadblock: RoadBlockGraphEdgeMapObject 30 | ) -> Tuple[List[LaneGraphEdgeMapObject], bool]: 31 | """ 32 | Performs dijkstra's shortest path to find a route to the target roadblock. 33 | :param target_roadblock: The target roadblock the path should end at. 34 | :return: 35 | - A route starting from the given start edge 36 | - A bool indicating if the route is successfully found. Successful means that there exists a path 37 | from the start edge to an edge contained in the end roadblock. 38 | If unsuccessful the shortest deepest path is returned. 39 | """ 40 | start_edge = self._queue[0] 41 | 42 | # Initial search states 43 | path_found: bool = False 44 | end_edge: LaneGraphEdgeMapObject = start_edge 45 | 46 | self._parent[start_edge.id] = None 47 | self._frontier = [start_edge.id] 48 | self._dist = [1] 49 | self._depth = [1] 50 | 51 | self._expanded = [] 52 | self._expanded_id = [] 53 | self._expanded_dist = [] 54 | self._expanded_depth = [] 55 | 56 | while len(self._queue) > 0: 57 | dist, idx = min((val, idx) for (idx, val) in enumerate(self._dist)) 58 | current_edge = self._queue[idx] 59 | current_depth = self._depth[idx] 60 | 61 | del self._dist[idx], self._queue[idx], self._frontier[idx], self._depth[idx] 62 | 63 | if self._check_goal_condition(current_edge, target_roadblock): 64 | end_edge = current_edge 65 | path_found = True 66 | break 67 | 68 | self._expanded.append(current_edge) 69 | self._expanded_id.append(current_edge.id) 70 | self._expanded_dist.append(dist) 71 | self._expanded_depth.append(current_depth) 72 | 73 | # Populate queue 74 | for next_edge in current_edge.outgoing_edges: 75 | if next_edge.id not in self._candidate_lane_edge_ids: 76 | continue 77 | 78 | alt = dist + self._edge_cost(next_edge) 79 | if ( 80 | next_edge.id not in self._expanded_id 81 | and next_edge.id not in self._frontier 82 | ): 83 | self._parent[next_edge.id] = current_edge 84 | self._queue.append(next_edge) 85 | self._frontier.append(next_edge.id) 86 | self._dist.append(alt) 87 | self._depth.append(current_depth + 1) 88 | end_edge = next_edge 89 | 90 | elif next_edge.id in self._frontier: 91 | next_edge_idx = self._frontier.index(next_edge.id) 92 | current_cost = self._dist[next_edge_idx] 93 | if alt < current_cost: 94 | self._parent[next_edge.id] = current_edge 95 | self._dist[next_edge_idx] = alt 96 | self._depth[next_edge_idx] = current_depth + 1 97 | 98 | if not path_found: 99 | # filter max depth 100 | max_depth = max(self._expanded_depth) 101 | idx_max_depth = list( 102 | np.where(np.array(self._expanded_depth) == max_depth)[0] 103 | ) 104 | dist_at_max_depth = [self._expanded_dist[i] for i in idx_max_depth] 105 | 106 | dist, _idx = min((val, idx) for (idx, val) in enumerate(dist_at_max_depth)) 107 | end_edge = self._expanded[idx_max_depth[_idx]] 108 | 109 | return self._construct_path(end_edge), path_found 110 | 111 | @staticmethod 112 | def _edge_cost(lane: LaneGraphEdgeMapObject) -> float: 113 | """ 114 | Edge cost of given lane. 115 | :param lane: lane class 116 | :return: length of lane 117 | """ 118 | return lane.baseline_path.length 119 | 120 | @staticmethod 121 | def _check_end_condition(depth: int, target_depth: int) -> bool: 122 | """ 123 | Check if the search should end regardless if the goal condition is met. 124 | :param depth: The current depth to check. 125 | :param target_depth: The target depth to check against. 126 | :return: True if: 127 | - The current depth exceeds the target depth. 128 | """ 129 | return depth > target_depth 130 | 131 | @staticmethod 132 | def _check_goal_condition( 133 | current_edge: LaneGraphEdgeMapObject, 134 | target_roadblock: RoadBlockGraphEdgeMapObject, 135 | ) -> bool: 136 | """ 137 | Check if the current edge is at the target roadblock at the given depth. 138 | :param current_edge: The edge to check. 139 | :param target_roadblock: The target roadblock the edge should be contained in. 140 | :return: whether the current edge is in the target roadblock 141 | """ 142 | return current_edge.get_roadblock_id() == target_roadblock.id 143 | 144 | def _construct_path( 145 | self, end_edge: LaneGraphEdgeMapObject 146 | ) -> List[LaneGraphEdgeMapObject]: 147 | """ 148 | :param end_edge: The end edge to start back propagating back to the start edge. 149 | :param depth: The depth of the target edge. 150 | :return: The constructed path as a list of LaneGraphEdgeMapObject 151 | """ 152 | path = [end_edge] 153 | while self._parent[end_edge.id] is not None: 154 | node = self._parent[end_edge.id] 155 | path.append(node) 156 | end_edge = node 157 | path.reverse() 158 | 159 | return path 160 | -------------------------------------------------------------------------------- /navsim/common/dataloader.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import lzma 4 | import pickle 5 | 6 | from pathlib import Path 7 | from typing import Any, Dict, List 8 | from tqdm import tqdm 9 | 10 | from navsim.common.dataclasses import AgentInput, Scene, SceneFilter, SensorConfig 11 | from navsim.planning.metric_caching.metric_cache import MetricCache 12 | import numpy as np 13 | 14 | def filter_scenes(data_path: Path, scene_filter: SceneFilter) -> Dict[str, List[Dict[str, Any]]]: 15 | 16 | def split_list(input_list: List[Any], num_frames: int, frame_interval: int) -> List[List[Any]]: 17 | return [input_list[i : i + num_frames] for i in range(0, len(input_list), frame_interval)] 18 | 19 | filtered_scenes: Dict[str, Scene] = {} 20 | stop_loading: bool = False 21 | 22 | # filter logs 23 | log_files = list(data_path.iterdir()) 24 | if scene_filter.log_names is not None: 25 | log_files = [ 26 | log_file 27 | for log_file in log_files 28 | if log_file.name.replace(".pkl", "") in scene_filter.log_names 29 | ] 30 | 31 | if scene_filter.tokens is not None: 32 | filter_tokens = True 33 | tokens = set(scene_filter.tokens) 34 | else: 35 | filter_tokens = False 36 | 37 | for log_pickle_path in tqdm(log_files, desc="Loading logs"): 38 | 39 | scene_dict_list = pickle.load(open(log_pickle_path, "rb")) 40 | for frame_list in split_list( 41 | scene_dict_list, scene_filter.num_frames, scene_filter.frame_interval 42 | ): 43 | # Filter scenes which are too short 44 | if len(frame_list) < scene_filter.num_frames: 45 | continue 46 | 47 | # Filter scenes with no route 48 | if ( 49 | scene_filter.has_route 50 | and len(frame_list[scene_filter.num_history_frames - 1]["roadblock_ids"]) == 0 51 | ): 52 | continue 53 | 54 | # Filter by token 55 | token = frame_list[scene_filter.num_history_frames - 1]["token"] 56 | if filter_tokens and token not in tokens: 57 | continue 58 | 59 | filtered_scenes[token] = frame_list 60 | 61 | if (scene_filter.max_scenes is not None) and ( 62 | len(filtered_scenes) >= scene_filter.max_scenes 63 | ): 64 | stop_loading = True 65 | break 66 | 67 | if stop_loading: 68 | break 69 | 70 | return filtered_scenes 71 | 72 | 73 | class SceneLoader: 74 | 75 | def __init__( 76 | self, 77 | data_path: Path, 78 | sensor_blobs_path: Path, 79 | scene_filter: SceneFilter, 80 | sensor_config: SensorConfig = SensorConfig.build_no_sensors(), 81 | train_debug: bool = False, 82 | ): 83 | if train_debug: 84 | print('begin loading scene_frames_dicts') 85 | self.scene_frames_dicts = np.load('/home/yingyan.li/repo/WoTE/dataset/scene_frames_dicts_debug.npy', allow_pickle=True).item() 86 | print('finish loading scene_frames_dicts') 87 | else: 88 | print('Begin filtering scenes') 89 | self.scene_frames_dicts = filter_scenes(data_path, scene_filter) 90 | print('Finish filtering Scenes ') 91 | self._sensor_blobs_path = sensor_blobs_path 92 | self._scene_filter = scene_filter 93 | self._sensor_config = sensor_config 94 | 95 | @property 96 | def tokens(self) -> List[str]: 97 | return list(self.scene_frames_dicts.keys()) 98 | 99 | def __len__(self): 100 | return len(self.tokens) 101 | 102 | def __getitem__(self, idx) -> str: 103 | return self.tokens[idx] 104 | 105 | def get_scene_from_token(self, token: str) -> Scene: 106 | assert token in self.tokens 107 | return Scene.from_scene_dict_list( 108 | self.scene_frames_dicts[token], 109 | self._sensor_blobs_path, 110 | num_history_frames=self._scene_filter.num_history_frames, 111 | num_future_frames=self._scene_filter.num_future_frames, 112 | sensor_config=self._sensor_config, 113 | ) 114 | 115 | def get_agent_input_from_token(self, token: str, use_fut_frames=False) -> AgentInput: 116 | assert token in self.tokens 117 | return AgentInput.from_scene_dict_list( 118 | self.scene_frames_dicts[token], 119 | self._sensor_blobs_path, 120 | num_history_frames=self._scene_filter.num_history_frames, 121 | sensor_config=self._sensor_config, 122 | use_fut_frames=use_fut_frames, 123 | ) 124 | 125 | def get_tokens_list_per_log(self) -> Dict[str, List[str]]: 126 | # generate a dict that contains a list of tokens for each log-name 127 | tokens_per_logs: Dict[str, List[str]] = {} 128 | for token, scene_dict_list in self.scene_frames_dicts.items(): 129 | log_name = scene_dict_list[0]["log_name"] 130 | if tokens_per_logs.get(log_name): 131 | tokens_per_logs[log_name].append(token) 132 | else: 133 | tokens_per_logs.update({log_name: [token]}) 134 | return tokens_per_logs 135 | 136 | class MetricCacheLoader: 137 | 138 | def __init__( 139 | self, 140 | cache_path: Path, 141 | file_name: str = "metric_cache.pkl", 142 | ): 143 | 144 | self._file_name = file_name 145 | self.metric_cache_paths = self._load_metric_cache_paths(cache_path) 146 | 147 | def _load_metric_cache_paths(self, cache_path: Path) -> Dict[str, Path]: 148 | metadata_dir = cache_path / "metadata" 149 | metadata_file = [file for file in metadata_dir.iterdir() if ".csv" in str(file)][0] 150 | with open(str(metadata_file), "r") as f: 151 | cache_paths=f.read().splitlines()[1:] 152 | metric_cache_dict = { 153 | cache_path.split("/")[-2]: cache_path 154 | for cache_path in cache_paths 155 | } 156 | return metric_cache_dict 157 | 158 | @property 159 | def tokens(self) -> List[str]: 160 | return list(self.metric_cache_paths.keys()) 161 | 162 | def __len__(self): 163 | return len(self.metric_cache_paths) 164 | 165 | def __getitem__(self, idx: int) -> MetricCache: 166 | return self.get_from_token(self.tokens[idx]) 167 | 168 | def get_from_token(self, token: str) -> MetricCache: 169 | 170 | with lzma.open(self.metric_cache_paths[token], "rb") as f: 171 | metric_cache: MetricCache = pickle.load(f) 172 | 173 | return metric_cache 174 | 175 | def to_pickle(self, path: Path) -> None: 176 | full_metric_cache = {} 177 | for token in tqdm(self.tokens): 178 | full_metric_cache[token] = self.get_from_token(token) 179 | with open(path, "wb") as f: 180 | pickle.dump(full_metric_cache, f) 181 | -------------------------------------------------------------------------------- /navsim/planning/script/utils.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | from dataclasses import dataclass 4 | from pathlib import Path 5 | from typing import List, Any 6 | 7 | import pandas as pd 8 | from omegaconf import DictConfig, OmegaConf 9 | 10 | from nuplan.common.utils.io_utils import safe_path_to_string 11 | from nuplan.common.utils.file_backed_barrier import distributed_sync 12 | from nuplan.planning.script.builders.folder_builder import build_simulation_experiment_folder 13 | from nuplan.planning.script.builders.logging_builder import build_logger 14 | from nuplan.planning.script.builders.main_callback_builder import build_main_multi_callback 15 | from nuplan.planning.simulation.main_callback.multi_main_callback import MultiMainCallback 16 | from nuplan.planning.simulation.runner.abstract_runner import AbstractRunner 17 | from nuplan.planning.simulation.runner.executor import execute_runners 18 | from nuplan.planning.simulation.runner.runner_report import RunnerReport 19 | from nuplan.planning.utils.multithreading.worker_pool import WorkerPool 20 | 21 | from navsim.planning.script.builders.worker_pool_builder import build_worker 22 | 23 | 24 | logger = logging.getLogger(__name__) 25 | 26 | @dataclass 27 | class CommonBuilder: 28 | """Common builder data.""" 29 | 30 | worker: WorkerPool 31 | multi_main_callback: MultiMainCallback 32 | output_dir: Path 33 | profiler: Any 34 | 35 | 36 | def update_config_for_simulation(cfg: DictConfig) -> None: 37 | """ 38 | Updates the config based on some conditions. 39 | :param cfg: DictConfig. Configuration that is used to run the experiment. 40 | """ 41 | # Make the configuration editable. 42 | OmegaConf.set_struct(cfg, False) 43 | if cfg.max_number_of_workers: 44 | # In case simulation is running in multi-threaded way perform the following 45 | # Remove the locking bottleneck 46 | cfg.callbacks = [callback for callback in cfg.callback.values()] 47 | 48 | # Save all interpolations and remove keys that were only used for interpolation and have no further use. 49 | OmegaConf.resolve(cfg) 50 | 51 | # Finalize the configuration and make it non-editable. 52 | OmegaConf.set_struct(cfg, True) 53 | 54 | # Log the final configuration after all overrides, interpolations and updates. 55 | if cfg.log_config: 56 | logger.info(f"Creating experiment: {cfg.experiment}") 57 | logger.info("\n" + OmegaConf.to_yaml(cfg)) 58 | 59 | def set_up_common_builder(cfg: DictConfig, profiler_name: str) -> CommonBuilder: 60 | """ 61 | Set up a common builder when running simulations. 62 | :param cfg: Hydra configuration. 63 | :param profiler_name: Profiler name. 64 | :return A data classes with common builders. 65 | """ 66 | # Build multi main callback 67 | multi_main_callback = build_main_multi_callback(cfg) 68 | 69 | # After run_simulation start 70 | multi_main_callback.on_run_simulation_start() 71 | 72 | # Update and override configs for simulation 73 | update_config_for_simulation(cfg=cfg) 74 | 75 | # Configure logger 76 | build_logger(cfg) 77 | 78 | # Construct builder 79 | worker = build_worker(cfg) 80 | 81 | # Create output storage folder 82 | build_simulation_experiment_folder(cfg=cfg) 83 | 84 | # Simulation Callbacks 85 | output_dir = Path(cfg.output_dir) 86 | 87 | return CommonBuilder( 88 | worker=worker, 89 | multi_main_callback=multi_main_callback, 90 | output_dir=output_dir, 91 | profiler=None, 92 | ) 93 | 94 | def set_default_path() -> None: 95 | """ 96 | This function sets the default paths as environment variables if none are set. 97 | These can then be used by Hydra, unless the user overwrites them from the command line. 98 | """ 99 | DEFAULT_DATA_ROOT = os.path.expanduser('~/nuplan/dataset') 100 | DEFAULT_EXP_ROOT = os.path.expanduser('~/nuplan/exp') 101 | 102 | if 'NUPLAN_DATA_ROOT' not in os.environ: 103 | logger.info(f'Setting default NUPLAN_DATA_ROOT: {DEFAULT_DATA_ROOT}') 104 | os.environ['NUPLAN_DATA_ROOT'] = DEFAULT_DATA_ROOT 105 | 106 | if 'NUPLAN_EXP_ROOT' not in os.environ: 107 | logger.info(f'Setting default NUPLAN_EXP_ROOT: {DEFAULT_EXP_ROOT}') 108 | os.environ['NUPLAN_EXP_ROOT'] = DEFAULT_EXP_ROOT 109 | 110 | def run_runners( 111 | runners: List[AbstractRunner], common_builder: CommonBuilder, profiler_name: str, cfg: DictConfig 112 | ) -> None: 113 | """ 114 | Run a list of runners. 115 | :param runners: A list of runners. 116 | :param common_builder: Common builder. 117 | :param profiler_name: Profiler name. 118 | :param cfg: Hydra config. 119 | """ 120 | assert len(runners) > 0, 'No scenarios found to simulate!' 121 | if common_builder.profiler: 122 | # Start simulation running profiling 123 | common_builder.profiler.start_profiler(profiler_name) 124 | 125 | logger.info('Executing runners...') 126 | reports = execute_runners( 127 | runners=runners, 128 | worker=common_builder.worker, 129 | num_gpus=cfg.number_of_gpus_allocated_per_simulation, 130 | num_cpus=cfg.number_of_cpus_allocated_per_simulation, 131 | exit_on_failure=cfg.exit_on_failure, 132 | verbose=cfg.verbose, 133 | ) 134 | logger.info('Finished executing runners!') 135 | 136 | # Save RunnerReports as parquet file 137 | save_runner_reports(reports, common_builder.output_dir, cfg.runner_report_file) 138 | 139 | # Sync up nodes when running distributed simulation 140 | distributed_sync(Path(cfg.output_dir / Path("barrier")), cfg.distributed_timeout_seconds) 141 | 142 | # Only run on_run_simulation_end callbacks on master node 143 | if int(os.environ.get('NODE_RANK', 0)) == 0: 144 | common_builder.multi_main_callback.on_run_simulation_end() 145 | 146 | # Save profiler 147 | if common_builder.profiler: 148 | common_builder.profiler.save_profiler(profiler_name) 149 | 150 | def save_runner_reports(reports: List[RunnerReport], output_dir: Path, report_name: str) -> None: 151 | """ 152 | Save runner reports to a parquet file in the output directory. 153 | Output directory can be local or s3. 154 | :param reports: Runner reports returned from each simulation. 155 | :param output_dir: Output directory to save the report. 156 | :param report_name: Report name. 157 | """ 158 | report_dicts = [] 159 | for report in map(lambda x: x.__dict__, reports): # type: ignore 160 | if (planner_report := report["planner_report"]) is not None: 161 | planner_report_statistics = planner_report.compute_summary_statistics() 162 | del report["planner_report"] 163 | report.update(planner_report_statistics) 164 | report_dicts.append(report) 165 | df = pd.DataFrame(report_dicts) 166 | df['duration'] = df['end_time'] - df['start_time'] 167 | 168 | save_path = output_dir / report_name 169 | df.to_parquet(safe_path_to_string(save_path)) 170 | logger.info(f'Saved runner reports to {save_path}') -------------------------------------------------------------------------------- /navsim/agents/SeerDrive/modules/multimodal_loss.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | import functools 5 | from typing import Callable, Optional 6 | from torch import Tensor 7 | 8 | 9 | def reduce_loss(loss: Tensor, reduction: str) -> Tensor: 10 | """Reduce loss as specified. 11 | 12 | Args: 13 | loss (Tensor): Elementwise loss tensor. 14 | reduction (str): Options are "none", "mean" and "sum". 15 | 16 | Return: 17 | Tensor: Reduced loss tensor. 18 | """ 19 | reduction_enum = F._Reduction.get_enum(reduction) 20 | # none: 0, elementwise_mean:1, sum: 2 21 | if reduction_enum == 0: 22 | return loss 23 | elif reduction_enum == 1: 24 | return loss.mean() 25 | elif reduction_enum == 2: 26 | return loss.sum() 27 | 28 | 29 | def weight_reduce_loss(loss: Tensor, 30 | weight: Optional[Tensor] = None, 31 | reduction: str = 'mean', 32 | avg_factor: Optional[float] = None) -> Tensor: 33 | """Apply element-wise weight and reduce loss. 34 | 35 | Args: 36 | loss (Tensor): Element-wise loss. 37 | weight (Optional[Tensor], optional): Element-wise weights. 38 | Defaults to None. 39 | reduction (str, optional): Same as built-in losses of PyTorch. 40 | Defaults to 'mean'. 41 | avg_factor (Optional[float], optional): Average factor when 42 | computing the mean of losses. Defaults to None. 43 | 44 | Returns: 45 | Tensor: Processed loss values. 46 | """ 47 | # if weight is specified, apply element-wise weight 48 | if weight is not None: 49 | loss = loss * weight 50 | 51 | # if avg_factor is not specified, just reduce the loss 52 | if avg_factor is None: 53 | loss = reduce_loss(loss, reduction) 54 | else: 55 | # if reduction is mean, then average the loss by avg_factor 56 | if reduction == 'mean': 57 | # Avoid causing ZeroDivisionError when avg_factor is 0.0, 58 | # i.e., all labels of an image belong to ignore index. 59 | eps = torch.finfo(torch.float32).eps 60 | loss = loss.sum() / (avg_factor + eps) 61 | # if reduction is 'none', then do nothing, otherwise raise an error 62 | elif reduction != 'none': 63 | raise ValueError('avg_factor can not be used with reduction="sum"') 64 | return loss 65 | 66 | 67 | def py_sigmoid_focal_loss(pred, 68 | target, 69 | weight=None, 70 | gamma=2.0, 71 | alpha=0.25, 72 | reduction='mean', 73 | avg_factor=None): 74 | """PyTorch version of `Focal Loss `_. 75 | 76 | Args: 77 | pred (torch.Tensor): The prediction with shape (N, C), C is the 78 | number of classes 79 | target (torch.Tensor): The learning label of the prediction. 80 | weight (torch.Tensor, optional): Sample-wise loss weight. 81 | gamma (float, optional): The gamma for calculating the modulating 82 | factor. Defaults to 2.0. 83 | alpha (float, optional): A balanced form for Focal Loss. 84 | Defaults to 0.25. 85 | reduction (str, optional): The method used to reduce the loss into 86 | a scalar. Defaults to 'mean'. 87 | avg_factor (int, optional): Average factor that is used to average 88 | the loss. Defaults to None. 89 | """ 90 | pred_sigmoid = pred.sigmoid() 91 | target = target.type_as(pred) 92 | # Actually, pt here denotes (1 - pt) in the Focal Loss paper 93 | pt = (1 - pred_sigmoid) * target + pred_sigmoid * (1 - target) 94 | # Thus it's pt.pow(gamma) rather than (1 - pt).pow(gamma) 95 | focal_weight = (alpha * target + (1 - alpha) * 96 | (1 - target)) * pt.pow(gamma) 97 | loss = F.binary_cross_entropy_with_logits( 98 | pred, target, reduction='none') * focal_weight 99 | if weight is not None: 100 | if weight.shape != loss.shape: 101 | if weight.size(0) == loss.size(0): 102 | # For most cases, weight is of shape (num_priors, ), 103 | # which means it does not have the second axis num_class 104 | weight = weight.view(-1, 1) 105 | else: 106 | # Sometimes, weight per anchor per class is also needed. e.g. 107 | # in FSAF. But it may be flattened of shape 108 | # (num_priors x num_class, ), while loss is still of shape 109 | # (num_priors, num_class). 110 | assert weight.numel() == loss.numel() 111 | weight = weight.view(loss.size(0), -1) 112 | assert weight.ndim == loss.ndim 113 | loss = weight_reduce_loss(loss, weight, reduction, avg_factor) 114 | return loss 115 | 116 | 117 | class DDLossComputer(nn.Module): 118 | def __init__(self, config): 119 | self._config = config 120 | super(DDLossComputer, self).__init__() 121 | # self.focal_loss = FocalLoss(use_sigmoid=True, gamma=2.0, alpha=0.25, reduction='mean', loss_weight=1.0, activated=False) 122 | self.cls_loss_weight = config.trajectory_cls_weight 123 | self.reg_loss_weight = config.trajectory_reg_weight 124 | 125 | def forward(self, poses_reg, poses_cls, targets, plan_anchor): 126 | """ 127 | pred_traj: (bs, 20, 8, 3) 128 | pred_cls: (bs, 20) 129 | plan_anchor: (bs,20, 8, 2) 130 | targets['trajectory']: (bs, 8, 3) 131 | """ 132 | bs, num_mode, ts, d = poses_reg.shape 133 | target_traj = targets["trajectory"] 134 | dist = torch.linalg.norm(target_traj[...,:2] - plan_anchor, dim=-1) 135 | dist = dist.mean(dim=-1) 136 | mode_idx = torch.argmin(dist, dim=-1) 137 | cls_target = mode_idx 138 | mode_idx = mode_idx[...,None,None,None].repeat(1,1,ts,d) 139 | best_reg = torch.gather(poses_reg, 1, mode_idx).squeeze(1) 140 | # import ipdb; ipdb.set_trace() 141 | # Calculate cls loss using focal loss 142 | target_classes_onehot = torch.zeros([bs, num_mode], 143 | dtype=poses_cls.dtype, 144 | layout=poses_cls.layout, 145 | device=poses_cls.device) 146 | target_classes_onehot.scatter_(1, cls_target.unsqueeze(1), 1) 147 | 148 | # Use py_sigmoid_focal_loss function for focal loss calculation 149 | loss_cls = self.cls_loss_weight * py_sigmoid_focal_loss( 150 | poses_cls, 151 | target_classes_onehot, 152 | weight=None, 153 | gamma=2.0, 154 | alpha=0.25, 155 | reduction='mean', 156 | avg_factor=None 157 | ) 158 | 159 | # Calculate regression loss 160 | reg_loss = self.reg_loss_weight * F.l1_loss(best_reg, target_traj.squeeze(1)) 161 | # import ipdb; ipdb.set_trace() 162 | # Combine classification and regression losses 163 | ret_loss = loss_cls + reg_loss 164 | return ret_loss 165 | -------------------------------------------------------------------------------- /navsim/planning/simulation/planner/pdm_planner/abstract_pdm_closed_planner.py: -------------------------------------------------------------------------------- 1 | from typing import List, Optional 2 | 3 | import numpy as np 4 | from nuplan.common.actor_state.ego_state import EgoState 5 | from nuplan.common.maps.abstract_map_objects import LaneGraphEdgeMapObject 6 | from nuplan.planning.simulation.planner.abstract_planner import PlannerInput 7 | from nuplan.planning.simulation.trajectory.interpolated_trajectory import ( 8 | InterpolatedTrajectory, 9 | ) 10 | from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling 11 | 12 | from navsim.planning.simulation.planner.pdm_planner.abstract_pdm_planner import ( 13 | AbstractPDMPlanner, 14 | ) 15 | from navsim.planning.simulation.planner.pdm_planner.observation.pdm_observation import ( 16 | PDMObservation, 17 | ) 18 | from navsim.planning.simulation.planner.pdm_planner.proposal.batch_idm_policy import ( 19 | BatchIDMPolicy, 20 | ) 21 | from navsim.planning.simulation.planner.pdm_planner.proposal.pdm_generator import ( 22 | PDMGenerator, 23 | ) 24 | from navsim.planning.simulation.planner.pdm_planner.proposal.pdm_proposal import ( 25 | PDMProposalManager, 26 | ) 27 | from navsim.planning.simulation.planner.pdm_planner.scoring.pdm_scorer import ( 28 | PDMScorer, 29 | ) 30 | from navsim.planning.simulation.planner.pdm_planner.simulation.pdm_simulator import ( 31 | PDMSimulator, 32 | ) 33 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_emergency_brake import ( 34 | PDMEmergencyBrake, 35 | ) 36 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_geometry_utils import ( 37 | parallel_discrete_path, 38 | ) 39 | from navsim.planning.simulation.planner.pdm_planner.utils.pdm_path import PDMPath 40 | 41 | 42 | class AbstractPDMClosedPlanner(AbstractPDMPlanner): 43 | """ 44 | Interface for planners incorporating PDM-Closed. Used for PDM-Closed and PDM-Hybrid. 45 | """ 46 | 47 | def __init__( 48 | self, 49 | trajectory_sampling: TrajectorySampling, 50 | proposal_sampling: TrajectorySampling, 51 | idm_policies: BatchIDMPolicy, 52 | lateral_offsets: Optional[List[float]], 53 | map_radius: float, 54 | ): 55 | """ 56 | Constructor for AbstractPDMClosedPlanner 57 | :param trajectory_sampling: Sampling parameters for final trajectory 58 | :param proposal_sampling: Sampling parameters for proposals 59 | :param idm_policies: BatchIDMPolicy class 60 | :param lateral_offsets: centerline offsets for proposals (optional) 61 | :param map_radius: radius around ego to consider 62 | """ 63 | 64 | super(AbstractPDMClosedPlanner, self).__init__(map_radius) 65 | 66 | assert ( 67 | trajectory_sampling.interval_length == proposal_sampling.interval_length 68 | ), "AbstractPDMClosedPlanner: Proposals and Trajectory must have equal interval length!" 69 | 70 | # config parameters 71 | self._trajectory_sampling: int = trajectory_sampling 72 | self._proposal_sampling: int = proposal_sampling 73 | self._idm_policies: BatchIDMPolicy = idm_policies 74 | self._lateral_offsets: Optional[List[float]] = lateral_offsets 75 | 76 | # observation/forecasting class 77 | self._observation = PDMObservation(trajectory_sampling, proposal_sampling, map_radius) 78 | 79 | # proposal/trajectory related classes 80 | self._generator = PDMGenerator(trajectory_sampling, proposal_sampling) 81 | self._simulator = PDMSimulator(proposal_sampling) 82 | self._scorer = PDMScorer(proposal_sampling) 83 | 84 | # lazy loaded 85 | self._proposal_manager: Optional[PDMProposalManager] = None 86 | 87 | def _update_proposal_manager(self, ego_state: EgoState): 88 | """ 89 | Updates or initializes PDMProposalManager class 90 | :param ego_state: state of ego-vehicle 91 | """ 92 | 93 | current_lane = self._get_starting_lane(ego_state) 94 | 95 | # TODO: Find additional conditions to trigger re-planning 96 | create_new_proposals = self._iteration == 0 97 | 98 | if create_new_proposals: 99 | proposal_paths: List[PDMPath] = self._get_proposal_paths(current_lane) 100 | 101 | self._proposal_manager = PDMProposalManager( 102 | lateral_proposals=proposal_paths, 103 | longitudinal_policies=self._idm_policies, 104 | ) 105 | 106 | # update proposals 107 | self._proposal_manager.update(current_lane.speed_limit_mps) 108 | 109 | def _get_proposal_paths(self, current_lane: LaneGraphEdgeMapObject) -> List[PDMPath]: 110 | """ 111 | Returns a list of path's to follow for the proposals. Inits a centerline. 112 | :param current_lane: current or starting lane of path-planning 113 | :return: lists of paths (0-index is centerline) 114 | """ 115 | centerline_discrete_path = self._get_discrete_centerline(current_lane) 116 | self._centerline = PDMPath(centerline_discrete_path) 117 | 118 | # 1. save centerline path (necessary for progress metric) 119 | output_paths: List[PDMPath] = [self._centerline] 120 | 121 | # 2. add additional paths with lateral offset of centerline 122 | if self._lateral_offsets is not None: 123 | for lateral_offset in self._lateral_offsets: 124 | offset_discrete_path = parallel_discrete_path( 125 | discrete_path=centerline_discrete_path, offset=lateral_offset 126 | ) 127 | output_paths.append(PDMPath(offset_discrete_path)) 128 | 129 | return output_paths 130 | 131 | def _get_closed_loop_trajectory( 132 | self, 133 | current_input: PlannerInput, 134 | ) -> InterpolatedTrajectory: 135 | """ 136 | Creates the closed-loop trajectory for PDM-Closed planner. 137 | :param current_input: planner input 138 | :return: trajectory 139 | """ 140 | 141 | ego_state, observation = current_input.history.current_state 142 | 143 | # 1. Environment forecast and observation update 144 | self._observation.update( 145 | ego_state, 146 | observation, 147 | current_input.traffic_light_data, 148 | self._route_lane_dict, 149 | ) 150 | 151 | # 2. Centerline extraction and proposal update 152 | self._update_proposal_manager(ego_state) 153 | 154 | # 3. Generate/Unroll proposals 155 | proposals_array = self._generator.generate_proposals( 156 | ego_state, self._observation, self._proposal_manager 157 | ) 158 | 159 | # 4. Simulate proposals 160 | simulated_proposals_array = self._simulator.simulate_proposals(proposals_array, ego_state) 161 | 162 | # 5. Score proposals 163 | proposal_scores = self._scorer.score_proposals( 164 | simulated_proposals_array, 165 | self._observation, 166 | self._centerline, 167 | list(self._route_lane_dict.keys()), 168 | self._drivable_area_map, 169 | ) 170 | 171 | trajectory = self._generator.generate_trajectory(np.argmax(proposal_scores)) 172 | return trajectory 173 | --------------------------------------------------------------------------------