Source code for olo.navigation.types

"""Navigation types for goals and state."""

from dataclasses import dataclass, field
from enum import Enum, IntEnum

from olo.spatial import Pose




class Availability(Enum):
    """Whether NavigateToPose is available for a namespace."""

    UNSPECIFIED = "unspecified"
    UNAVAILABLE = "unavailable"
    READY = "ready"





class Activity(Enum):
    """Whether this executor currently owns an active navigation goal."""

    UNSPECIFIED = "unspecified"
    IDLE = "idle"
    NAVIGATING = "navigating"





class NavigationOutcome(Enum):
    """Terminal outcome for a navigation goal."""

    UNSPECIFIED = "unspecified"
    SUCCEEDED = "succeeded"
    CANCELLED = "cancelled"
    ABORTED = "aborted"
    TIMED_OUT = "timed_out"



class Nav2ErrorCode(IntEnum):
    """Nav2 ``NavigateToPose.Result.error_code`` values.

    ``NavigateToPose`` itself only defines ``NONE``; a failure propagates the
    code of whichever subtask failed, so every reachable ``nav2_msgs`` range is
    listed here. Names carry a task prefix because ``UNKNOWN``, ``TF_ERROR``,
    and ``TIMEOUT`` repeat across ranges.
    """

    NONE = 0

    # FollowPath (controller)
    CONTROLLER_UNKNOWN = 100
    INVALID_CONTROLLER = 101
    CONTROLLER_TF_ERROR = 102
    CONTROLLER_INVALID_PATH = 103
    PATIENCE_EXCEEDED = 104
    FAILED_TO_MAKE_PROGRESS = 105
    NO_VALID_CONTROL = 106
    CONTROLLER_TIMED_OUT = 107

    # ComputePathToPose (planner)
    PLANNER_UNKNOWN = 200
    INVALID_PLANNER = 201
    PLANNER_TF_ERROR = 202
    START_OUTSIDE_MAP = 203
    GOAL_OUTSIDE_MAP = 204
    START_OCCUPIED = 205
    GOAL_OCCUPIED = 206
    PLANNER_TIMEOUT = 207
    NO_VALID_PATH = 208

    # ComputePathThroughPoses
    PLANNER_POSES_UNKNOWN = 300
    PLANNER_POSES_INVALID_PLANNER = 301
    PLANNER_POSES_TF_ERROR = 302
    PLANNER_POSES_START_OUTSIDE_MAP = 303
    PLANNER_POSES_GOAL_OUTSIDE_MAP = 304
    PLANNER_POSES_START_OCCUPIED = 305
    PLANNER_POSES_GOAL_OCCUPIED = 306
    PLANNER_POSES_TIMEOUT = 307
    PLANNER_POSES_NO_VALID_PATH = 308
    NO_VIAPOINTS_GIVEN = 309

    # SmoothPath
    SMOOTHER_UNKNOWN = 500
    INVALID_SMOOTHER = 501
    SMOOTHER_TIMEOUT = 502
    SMOOTHED_PATH_IN_COLLISION = 503
    FAILED_TO_SMOOTH_PATH = 504
    SMOOTHER_INVALID_PATH = 505

    # FollowWaypoints
    WAYPOINT_UNKNOWN = 600
    TASK_EXECUTOR_FAILED = 601

    # Recovery behaviours
    SPIN_UNKNOWN = 700
    SPIN_TIMEOUT = 701
    SPIN_TF_ERROR = 702
    SPIN_COLLISION_AHEAD = 703
    BACKUP_UNKNOWN = 710
    BACKUP_TIMEOUT = 711
    BACKUP_TF_ERROR = 712
    BACKUP_INVALID_INPUT = 713
    BACKUP_COLLISION_AHEAD = 714
    DRIVE_ON_HEADING_UNKNOWN = 720
    DRIVE_ON_HEADING_TIMEOUT = 721
    DRIVE_ON_HEADING_TF_ERROR = 722
    DRIVE_ON_HEADING_COLLISION_AHEAD = 723
    DRIVE_ON_HEADING_INVALID_INPUT = 724
    ASSISTED_TELEOP_UNKNOWN = 730
    ASSISTED_TELEOP_TIMEOUT = 731
    ASSISTED_TELEOP_TF_ERROR = 732

    @classmethod
    def name_for(cls, code: int) -> str:
        """Return a readable label for a Nav2 error code."""
        try:
            return cls(code).name
        except ValueError:
            return f"UNKNOWN({code})"




@dataclass(frozen=True, slots=True)
class ReachabilityResult:
    """Planner-backed reachability for a map-frame goal."""

    reachable: bool = False
    planner_error_code: int = 0
    reason: str = ""
    path_length: float = 0.0





@dataclass(frozen=True, slots=True)
class NavigationState:
    """Capability snapshot for a robot namespace."""

    availability: Availability = Availability.UNSPECIFIED
    activity: Activity = Activity.UNSPECIFIED
    owned_goal_ids: tuple[str, ...] = ()
    reason: str = ""

    @property
    def ready(self) -> bool:
        """Return whether NavigateToPose is ready."""
        return self.availability is Availability.READY





@dataclass(frozen=True, slots=True)
class NavigationFeedback:
    """Progress update for an active navigation goal."""

    distance_remaining: float = 0.0
    navigation_time: float = 0.0
    number_of_recoveries: int = 0
    current_pose: Pose = field(default_factory=Pose)





@dataclass(frozen=True, slots=True)
class NavigationResult:
    """Terminal result for a navigation goal."""

    outcome: NavigationOutcome = NavigationOutcome.UNSPECIFIED
    reason: str = ""
    nav2_error_code: int = 0

    @property
    def nav2_error(self) -> Nav2ErrorCode | None:
        """Return the Nav2 error code when present."""
        if self.nav2_error_code == 0:
            return None
        try:
            return Nav2ErrorCode(self.nav2_error_code)
        except ValueError:
            return None



__all__ = [
    "Activity",
    "Availability",
    "Nav2ErrorCode",
    "NavigationFeedback",
    "NavigationOutcome",
    "NavigationResult",
    "NavigationState",
    "ReachabilityResult",
]