Motivation
The current Planner component returns a single "most optimal" path solution for a given static map, start and goal. While this is sufficient for standalone point-to-point navigation, it is limiting when the planner is driven by a higher-level reasoning or task-planning component.
A higher-level system may want to:
- Reject the current plan because it passes through an area with a reported incident, crowd, or temporary hazard.
- Choose a longer but safer / wider / less-trafficked route.
- Compare several candidate routes against task-level criteria (risk, semantic zones) before committing.
- Offer the user alternative routes (similar to consumer navigation apps).
Today none of this is possible without throwing the problem away and reconfiguring the algorithm used in OMPL from scratch, and even then there is no guarantee the next solution will be different from the previous one.
Proposed approach
Phase 1: Expose alternative solutions already computed by OMPL
OMPL's ProblemDefinition internally stores every solution found during a solve() call. With anytime optimizing planners (RRT*, BIT*, AIT*, PRM*, InformedRRT*, …), this means each improvement over the planning horizon is retained as a PlannerSolution with its own cost and length. Today the bindings in kompass-core only surface the final best one.
Binding changes:
- Bind
ob::ProblemDefinition with getSolutionCount() and getSolutions().
- Bind
ob::PlannerSolution exposing path_, cost_, length_, optimized_.
- Bind
og::SimpleSetup::getProblemDefinition().
Python API:
- New method
solve_multiple(max_paths: int) -> list[PathGeometric] that runs solve() with the configured timeout and returns all retained solutions sorted by cost.
- Each returned path is annotated with its cost so the caller can reason about the trade-off.
Pros: cheap, no extra planning cost, just surfacing data OMPL already has.
Cons: the alternatives tend to be topologically similar (all converging toward the optimum in the same homotopy class).
Phase 2: Replan while avoiding a region flagged by the higher-level system
When the reasoning layer says "this plan is unacceptable because it passes through X", the planner should be able to replan with that region treated as a virtual obstacle.
Python API:
- New method
replan_avoiding(regions: list[AvoidanceRegion]) -> PathGeometric that:
- Wraps the existing state validity checker with an additional predicate that rejects states falling inside any
AvoidanceRegion (circle or polygon).
- Calls
SimpleSetup::clear() and re-solves.
- Restores the original validity checker afterwards.
- An
AvoidanceRegion dataclass capturing shape + metadata (reason, source, expiry).
This gives the task planner a clean "avoid here, give me another path" primitive, which is the actual operation a higher-level reasoner wants.
Acceptance criteria
Motivation
The current
Plannercomponent returns a single "most optimal" path solution for a given static map, start and goal. While this is sufficient for standalone point-to-point navigation, it is limiting when the planner is driven by a higher-level reasoning or task-planning component.A higher-level system may want to:
Today none of this is possible without throwing the problem away and reconfiguring the algorithm used in OMPL from scratch, and even then there is no guarantee the next solution will be different from the previous one.
Proposed approach
Phase 1: Expose alternative solutions already computed by OMPL
OMPL's
ProblemDefinitioninternally stores every solution found during asolve()call. With anytime optimizing planners (RRT*,BIT*,AIT*,PRM*,InformedRRT*, …), this means each improvement over the planning horizon is retained as aPlannerSolutionwith its own cost and length. Today the bindings in kompass-core only surface the final best one.Binding changes:
ob::ProblemDefinitionwithgetSolutionCount()andgetSolutions().ob::PlannerSolutionexposingpath_,cost_,length_,optimized_.og::SimpleSetup::getProblemDefinition().Python API:
solve_multiple(max_paths: int) -> list[PathGeometric]that runssolve()with the configured timeout and returns all retained solutions sorted by cost.Pros: cheap, no extra planning cost, just surfacing data OMPL already has.
Cons: the alternatives tend to be topologically similar (all converging toward the optimum in the same homotopy class).
Phase 2: Replan while avoiding a region flagged by the higher-level system
When the reasoning layer says "this plan is unacceptable because it passes through X", the planner should be able to replan with that region treated as a virtual obstacle.
Python API:
replan_avoiding(regions: list[AvoidanceRegion]) -> PathGeometricthat:AvoidanceRegion(circle or polygon).SimpleSetup::clear()and re-solves.AvoidanceRegiondataclass capturing shape + metadata (reason, source, expiry).This gives the task planner a clean "avoid here, give me another path" primitive, which is the actual operation a higher-level reasoner wants.
Acceptance criteria
solve_multiple(k)returns up toksolutions with associated costs.replan_avoiding(regions)returns a path that does not intersect any provided region, or raises if infeasible.replan_avoiding.