|
import logging
|
|
import os
|
|
import pickle
|
|
from pathlib import Path
|
|
from typing import Any, Dict, List, Tuple
|
|
|
|
import hydra
|
|
import numpy as np
|
|
import pandas as pd
|
|
import torch
|
|
from hydra.utils import instantiate
|
|
from nuplan.planning.script.builders.logging_builder import build_logger
|
|
from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling
|
|
from nuplan.planning.utils.multithreading.worker_utils import worker_map
|
|
from omegaconf import DictConfig
|
|
|
|
from navsim.common.dataclasses import Trajectory
|
|
from navsim.common.dataloader import SceneLoader
|
|
from navsim.planning.script.builders.worker_pool_builder import build_worker
|
|
from navsim.planning.script.run_pdm_score_gpu import run_pdm_score
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
CONFIG_PATH = "config/pdm_scoring"
|
|
CONFIG_NAME = "run_pdm_score_ddp"
|
|
|
|
"""
|
|
pkl -> valid score
|
|
"""
|
|
|
|
|
|
@hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME)
|
|
def main(cfg: DictConfig) -> None:
|
|
pkl_path = cfg.pkl_path
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
imi_weight = 0.01
|
|
noc_weight = 0.1
|
|
da_weight = 0.1
|
|
ttc_weight = 5.0
|
|
progress_weight = 5.0
|
|
comfort_weight = 2.0
|
|
tpc_weight = 6.0
|
|
|
|
|
|
imi_weight = 0.015
|
|
noc_weight = 0.5
|
|
da_weight = 0.82
|
|
tpc_weight = 3.6
|
|
|
|
merged_predictions = pickle.load(open(pkl_path, 'rb'))
|
|
traj_vocab = np.load(f'{os.getenv("NAVSIM_DEVKIT_ROOT")}/traj_final/test_8192_kmeans.npy')
|
|
for k, v in merged_predictions.items():
|
|
score = (
|
|
imi_weight * torch.from_numpy(v['imi']) +
|
|
noc_weight * torch.from_numpy(v['noc']) +
|
|
da_weight * torch.from_numpy(v['da']) +
|
|
tpc_weight * (
|
|
ttc_weight * torch.exp(torch.from_numpy(v['ttc'])) +
|
|
comfort_weight * torch.exp(torch.from_numpy(v['comfort'])) +
|
|
progress_weight * torch.exp(torch.from_numpy(v['progress']))
|
|
).log()
|
|
).argmax(0).item()
|
|
traj = traj_vocab[score]
|
|
merged_predictions[k]['trajectory'] = Trajectory(traj,
|
|
TrajectorySampling(
|
|
time_horizon=4,
|
|
interval_length=0.1))
|
|
|
|
build_logger(cfg)
|
|
scene_filter = instantiate(cfg.scene_filter)
|
|
scene_loader = SceneLoader(
|
|
sensor_blobs_path=Path(cfg.sensor_blobs_path),
|
|
data_path=Path(cfg.navsim_log_path),
|
|
scene_filter=scene_filter,
|
|
)
|
|
|
|
data_points = [
|
|
{
|
|
"cfg": cfg,
|
|
"log_file": log_file,
|
|
"tokens": tokens_list,
|
|
"model_trajectory": merged_predictions
|
|
}
|
|
for log_file, tokens_list in scene_loader.get_tokens_list_per_log().items()
|
|
]
|
|
total_token_cnt = sum([len(t["tokens"]) for t in data_points])
|
|
assert len(merged_predictions) == total_token_cnt, (f'merged: {len(merged_predictions)},'
|
|
f'total: {total_token_cnt}')
|
|
|
|
worker = build_worker(cfg)
|
|
score_rows: List[Tuple[Dict[str, Any], int, int]] = worker_map(worker, run_pdm_score, data_points)
|
|
pdm_score_df = pd.DataFrame(score_rows)
|
|
num_sucessful_scenarios = pdm_score_df["valid"].sum()
|
|
num_failed_scenarios = len(pdm_score_df) - num_sucessful_scenarios
|
|
average_row = pdm_score_df.drop(columns=["token", "valid"]).mean(skipna=True)
|
|
average_row["token"] = "average"
|
|
average_row["valid"] = pdm_score_df["valid"].all()
|
|
pdm_score_df.loc[len(pdm_score_df)] = average_row
|
|
|
|
save_path = Path(cfg.csv_path)
|
|
pdm_score_df.to_csv(save_path)
|
|
|
|
logger.info(f"""
|
|
Finished running evaluation.
|
|
Number of successful scenarios: {num_sucessful_scenarios}.
|
|
Number of failed scenarios: {num_failed_scenarios}.
|
|
Final average score of valid results: {pdm_score_df['score'].mean()}.
|
|
Results are stored in: {save_path}.
|
|
""")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|
