From b904a9957023277caf2c568757581d2acd0d148d Mon Sep 17 00:00:00 2001 From: shingo_yokoi Date: Sun, 23 Nov 2025 02:00:34 +0900 Subject: [PATCH] Parameterize route correction search depth to avoid infinite loops --- navsim/planning/metric_caching/caching.py | 2 ++ .../metric_caching/metric_cache_processor.py | 8 ++++++++ .../metric_caching/default_metric_caching.yaml | 4 ++++ .../pdm_planner/abstract_pdm_closed_planner.py | 10 +++++++++- .../planner/pdm_planner/abstract_pdm_planner.py | 14 +++++++++++++- .../planner/pdm_planner/pdm_closed_planner.py | 6 ++++++ 6 files changed, 42 insertions(+), 2 deletions(-) diff --git a/navsim/planning/metric_caching/caching.py b/navsim/planning/metric_caching/caching.py index feab5f4..63ca64e 100644 --- a/navsim/planning/metric_caching/caching.py +++ b/navsim/planning/metric_caching/caching.py @@ -91,6 +91,8 @@ def cache_single_synthetic_scenario( cache_path=cfg.metric_cache_path, force_feature_computation=cfg.force_feature_computation, proposal_sampling=instantiate(cfg.proposal_sampling), + pdm_search_depth_backward=cfg.pdm_search_depth_backward, + pdm_search_depth_forward=cfg.pdm_search_depth_forward, ) logger.info(f"Extracted {len(scene_loader)} scenarios for thread_id={thread_id}, node_id={node_id}.") diff --git a/navsim/planning/metric_caching/metric_cache_processor.py b/navsim/planning/metric_caching/metric_cache_processor.py index f436c07..60f50cd 100644 --- a/navsim/planning/metric_caching/metric_cache_processor.py +++ b/navsim/planning/metric_caching/metric_cache_processor.py @@ -38,14 +38,20 @@ def __init__( cache_path: Optional[str], force_feature_computation: bool, proposal_sampling: TrajectorySampling, + pdm_search_depth_backward: int = 15, + pdm_search_depth_forward: int = 30, ): """ Initialize class. :param cache_path: Whether to cache features. :param force_feature_computation: If true, even if cache exists, it will be overwritten. + :param pdm_search_depth_backward: depth of backward BFS search for route correction + :param pdm_search_depth_forward: depth of forward BFS search for route correction """ self._cache_path = pathlib.Path(cache_path) if cache_path else None self._force_feature_computation = force_feature_computation + self._pdm_search_depth_backward = pdm_search_depth_backward + self._pdm_search_depth_forward = pdm_search_depth_forward # 1s additional observation for ttc metric future_poses = proposal_sampling.num_poses + int(1.0 / proposal_sampling.interval_length) @@ -66,6 +72,8 @@ def __init__( ), lateral_offsets=[-1.0, 1.0], map_radius=self._map_radius, + pdm_search_depth_backward=self._pdm_search_depth_backward, + pdm_search_depth_forward=self._pdm_search_depth_forward, ) def _get_planner_inputs(self, scenario: AbstractScenario) -> Tuple[PlannerInput, PlannerInitialization]: diff --git a/navsim/planning/script/config/metric_caching/default_metric_caching.yaml b/navsim/planning/script/config/metric_caching/default_metric_caching.yaml index b0371ca..8055bcf 100644 --- a/navsim/planning/script/config/metric_caching/default_metric_caching.yaml +++ b/navsim/planning/script/config/metric_caching/default_metric_caching.yaml @@ -14,4 +14,8 @@ defaults: force_feature_computation: True +# PDM Planner Route Correction Parameters +pdm_search_depth_backward: 15 +pdm_search_depth_forward: 30 + output_dir: ${metric_cache_path}/metadata diff --git a/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_closed_planner.py b/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_closed_planner.py index c14eed2..7f49dc5 100644 --- a/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_closed_planner.py +++ b/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_closed_planner.py @@ -31,6 +31,8 @@ def __init__( idm_policies: BatchIDMPolicy, lateral_offsets: Optional[List[float]], map_radius: float, + pdm_search_depth_backward: int = 15, + pdm_search_depth_forward: int = 30, ): """ Constructor for AbstractPDMClosedPlanner @@ -39,9 +41,15 @@ def __init__( :param idm_policies: BatchIDMPolicy class :param lateral_offsets: centerline offsets for proposals (optional) :param map_radius: radius around ego to consider + :param pdm_search_depth_backward: depth of backward BFS search for route correction + :param pdm_search_depth_forward: depth of forward BFS search for route correction """ - super(AbstractPDMClosedPlanner, self).__init__(map_radius) + super(AbstractPDMClosedPlanner, self).__init__( + map_radius, + pdm_search_depth_backward=pdm_search_depth_backward, + pdm_search_depth_forward=pdm_search_depth_forward, + ) assert ( trajectory_sampling.interval_length == proposal_sampling.interval_length diff --git a/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_planner.py b/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_planner.py index edb6168..32a959e 100644 --- a/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_planner.py +++ b/navsim/planning/simulation/planner/pdm_planner/abstract_pdm_planner.py @@ -26,13 +26,19 @@ class AbstractPDMPlanner(AbstractPlanner, ABC): def __init__( self, map_radius: float, + pdm_search_depth_backward: int = 15, + pdm_search_depth_forward: int = 30, ): """ Constructor of AbstractPDMPlanner. :param map_radius: radius around ego to consider + :param pdm_search_depth_backward: depth of backward BFS search for route correction + :param pdm_search_depth_forward: depth of forward BFS search for route correction """ self._map_radius: int = map_radius # [m] + self._pdm_search_depth_backward = pdm_search_depth_backward + self._pdm_search_depth_forward = pdm_search_depth_forward self._iteration: int = 0 # lazy loaded @@ -68,7 +74,13 @@ def _route_roadblock_correction(self, ego_state: EgoState) -> None: Corrects the roadblock route and reloads lane-graph dictionaries. :param ego_state: state of the ego vehicle. """ - route_roadblock_ids = route_roadblock_correction(ego_state.rear_axle, self._map_api, self._route_roadblock_dict) + route_roadblock_ids = route_roadblock_correction( + ego_state.rear_axle, + self._map_api, + self._route_roadblock_dict, + search_depth_backward=self._pdm_search_depth_backward, + search_depth_forward=self._pdm_search_depth_forward, + ) self._load_route_dicts(route_roadblock_ids) def _get_discrete_centerline(self, current_lane: LaneGraphEdgeMapObject, search_depth: int = 30) -> List[StateSE2]: diff --git a/navsim/planning/simulation/planner/pdm_planner/pdm_closed_planner.py b/navsim/planning/simulation/planner/pdm_planner/pdm_closed_planner.py index 7dd5221..e3d35b7 100644 --- a/navsim/planning/simulation/planner/pdm_planner/pdm_closed_planner.py +++ b/navsim/planning/simulation/planner/pdm_planner/pdm_closed_planner.py @@ -30,6 +30,8 @@ def __init__( idm_policies: BatchIDMPolicy, lateral_offsets: Optional[List[float]], map_radius: float, + pdm_search_depth_backward: int = 15, + pdm_search_depth_forward: int = 30, ): """ Constructor for PDMClosedPlanner @@ -38,6 +40,8 @@ def __init__( :param idm_policies: BatchIDMPolicy class :param lateral_offsets: centerline offsets for proposals (optional) :param map_radius: radius around ego to consider + :param pdm_search_depth_backward: depth of backward BFS search for route correction + :param pdm_search_depth_forward: depth of forward BFS search for route correction """ super(PDMClosedPlanner, self).__init__( trajectory_sampling, @@ -45,6 +49,8 @@ def __init__( idm_policies, lateral_offsets, map_radius, + pdm_search_depth_backward, + pdm_search_depth_forward, ) def initialize(self, initialization: PlannerInitialization) -> None: