Skip to content

Add support for obtaining multiple global path alternatives from the Planner component #44

Description

@mkabtoul

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:
    1. Wraps the existing state validity checker with an additional predicate that rejects states falling inside any AvoidanceRegion (circle or polygon).
    2. Calls SimpleSetup::clear() and re-solves.
    3. 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

  • Adding new ROS2 services or component actions to cover the new features.
  • solve_multiple(k) returns up to k solutions with associated costs.
  • replan_avoiding(regions) returns a path that does not intersect any provided region, or raises if infeasible.
  • New unit tests covering: multi-solution retrieval, replanning around a circular avoidance region, and restoration of the original validity checker after replan_avoiding.
  • Documentation snippet in the Planner guide showing the higher-level-reasoning use case.

Activity

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Metadata

Metadata

Assignees

No one assigned

    Labels

    enhancementNew feature or request

    Projects

    No projects

      Milestone

      No milestone

      Relationships

      None yet

      Development

      No branches or pull requests

      Issue actions