"""Navigation wrapper over olo.navigation.v1.Navigation."""
from collections.abc import Callable, Iterator
from typing import Optional
import grpc
from olo_protos.navigation.v1 import navigation_pb2
from olo._convert import duration_to_seconds, seconds_to_duration
from olo.errors import OloFailedPrecondition, wrap_rpc_error
from olo.navigation.grid import OccupancyGrid, occupancy_grid_from_proto
from olo.navigation.types import (
Activity,
Availability,
Nav2ErrorCode,
NavigationFeedback,
NavigationOutcome,
NavigationResult,
NavigationState,
ReachabilityResult,
)
from olo.spatial import Pose, Transform
from olo.spatial._convert import pose_from_proto, pose_to_proto
from olo.utils.common import normalize_robot_namespace
_DEFAULT_TIMEOUT_SEC = 10.0
_DEFAULT_NAV_TIMEOUT_SEC = 120.0
_AVAILABILITY = {
navigation_pb2.AVAILABILITY_UNSPECIFIED: Availability.UNSPECIFIED,
navigation_pb2.AVAILABILITY_UNAVAILABLE: Availability.UNAVAILABLE,
navigation_pb2.AVAILABILITY_READY: Availability.READY,
}
_ACTIVITY = {
navigation_pb2.ACTIVITY_UNSPECIFIED: Activity.UNSPECIFIED,
navigation_pb2.ACTIVITY_IDLE: Activity.IDLE,
navigation_pb2.ACTIVITY_NAVIGATING: Activity.NAVIGATING,
}
_OUTCOME = {
navigation_pb2.NAVIGATION_OUTCOME_UNSPECIFIED: NavigationOutcome.UNSPECIFIED,
navigation_pb2.NAVIGATION_OUTCOME_SUCCEEDED: NavigationOutcome.SUCCEEDED,
navigation_pb2.NAVIGATION_OUTCOME_CANCELLED: NavigationOutcome.CANCELLED,
navigation_pb2.NAVIGATION_OUTCOME_ABORTED: NavigationOutcome.ABORTED,
navigation_pb2.NAVIGATION_OUTCOME_TIMED_OUT: NavigationOutcome.TIMED_OUT,
}
def _state_from_proto(state: navigation_pb2.NavigationState) -> NavigationState:
return NavigationState(
availability=_AVAILABILITY.get(state.availability, Availability.UNSPECIFIED),
activity=_ACTIVITY.get(state.activity, Activity.UNSPECIFIED),
owned_goal_ids=tuple(state.owned_goal_ids),
reason=state.reason,
)
def _reachability_from_proto(
result: navigation_pb2.ReachabilityResult,
) -> ReachabilityResult:
return ReachabilityResult(
reachable=bool(result.reachable),
planner_error_code=int(result.planner_error_code),
reason=result.reason,
path_length=float(result.path_length),
)
def _feedback_from_proto(feedback: navigation_pb2.NavigationFeedback) -> NavigationFeedback:
return NavigationFeedback(
distance_remaining=float(feedback.distance_remaining),
navigation_time=duration_to_seconds(feedback.navigation_time),
number_of_recoveries=int(feedback.number_of_recoveries),
current_pose=pose_from_proto(feedback.current_pose),
)
def _result_from_proto(result: navigation_pb2.NavigationResult) -> NavigationResult:
return NavigationResult(
outcome=_OUTCOME.get(result.outcome, NavigationOutcome.UNSPECIFIED),
reason=result.reason,
nav2_error_code=int(result.nav2_error_code),
)
[docs]
class NavigationGoal:
"""Handle for an accepted long-running navigation goal."""
[docs]
def __init__(self, client: "Navigation", goal_id: str) -> None:
"""Bind to a navigation client and server-generated goal id."""
self._client = client
self.id = goal_id
self._result: NavigationResult | None = None
[docs]
def wait(
self,
*,
on_feedback: Callable[[NavigationFeedback], None] | None = None,
timeout: float | None = None,
) -> NavigationResult:
"""Block until the goal reaches a terminal outcome."""
if self._result is not None:
return self._result
for event in self._client._watch(self.id, timeout=timeout):
if isinstance(event, NavigationFeedback):
if on_feedback is not None:
on_feedback(event)
continue
self._result = event
return event
raise TimeoutError(f"Timed out waiting for navigation goal {self.id}")
[docs]
def cancel(self, *, timeout: float = _DEFAULT_TIMEOUT_SEC) -> None:
"""Cancel this goal."""
self._client.cancel_navigation(self.id, timeout=timeout)
[docs]
class Navigation:
"""Sync wrapper for the navigation gRPC service."""
[docs]
def __init__(self, session) -> None:
"""Bind to a channel session."""
self._session = session
def _call(self, stub_method, request, timeout: float | None):
try:
return stub_method(request, timeout=timeout)
except grpc.RpcError as exc:
raise wrap_rpc_error(exc) from exc
[docs]
def state(
self,
robot_namespace: str = "",
*,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> NavigationState:
"""Fetch a live navigation capability snapshot."""
request = navigation_pb2.GetNavigationStateRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
)
response = self._call(
self._session.navigation_stub.GetNavigationState,
request,
timeout=timeout,
)
return _state_from_proto(response.state)
[docs]
def send_navigation_goal(
self,
target: Pose,
*,
robot_namespace: str = "",
frame: str = "map",
navigation_timeout: float = _DEFAULT_NAV_TIMEOUT_SEC,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> NavigationGoal:
"""Send a NavigateToPose goal and return a goal handle."""
request = navigation_pb2.StartNavigationRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
target=pose_to_proto(target),
frame_id=frame,
navigation_timeout=seconds_to_duration(navigation_timeout),
)
response = self._call(
self._session.navigation_stub.StartNavigation,
request,
timeout=timeout,
)
return NavigationGoal(self, response.goal_id)
[docs]
def navigate_to(
self,
target: Pose,
*,
robot_namespace: str = "",
frame: str = "map",
navigation_timeout: float = _DEFAULT_NAV_TIMEOUT_SEC,
on_feedback: Callable[[NavigationFeedback], None] | None = None,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> NavigationResult:
"""Start navigation and wait for the terminal outcome."""
goal = self.send_navigation_goal(
target,
robot_namespace=robot_namespace,
frame=frame,
navigation_timeout=navigation_timeout,
timeout=timeout,
)
return goal.wait(on_feedback=on_feedback)
[docs]
def cancel_navigation(
self,
goal_id: str,
*,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> None:
"""Cancel one owned navigation goal."""
request = navigation_pb2.CancelNavigationRequest(goal_id=goal_id)
self._call(self._session.navigation_stub.CancelNavigation, request, timeout=timeout)
[docs]
def cancel_all(
self,
robot_namespace: str = "",
*,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> tuple[str, ...]:
"""Cancel all owned navigation goals for a namespace."""
request = navigation_pb2.CancelAllRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
)
response = self._call(
self._session.navigation_stub.CancelAll,
request,
timeout=timeout,
)
return tuple(response.cancelled_goal_ids)
[docs]
def get_map(
self,
robot_namespace: str = "",
*,
topic: str = "map",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> OccupancyGrid:
"""Fetch one occupancy-grid snapshot from the robot map topic."""
request = navigation_pb2.GetMapRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
topic=topic,
timeout=seconds_to_duration(timeout),
)
response = self._call(
self._session.navigation_stub.GetMap,
request,
timeout=timeout,
)
return occupancy_grid_from_proto(response.grid)
[docs]
def check_reachability(
self,
goal: Pose,
*,
robot_namespace: str = "",
frame: str = "map",
start: Pose | None = None,
use_start: bool = False,
planner_id: str = "",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> ReachabilityResult:
"""Ask Nav2 whether a goal pose is planner-reachable."""
request = navigation_pb2.CheckReachabilityRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
goal=pose_to_proto(goal),
frame_id=frame,
use_start=use_start,
planner_id=planner_id,
timeout=seconds_to_duration(timeout),
)
if use_start and start is not None:
request.start.CopyFrom(pose_to_proto(start))
response = self._call(
self._session.navigation_stub.CheckReachability,
request,
timeout=timeout,
)
return _reachability_from_proto(response.result)
def _watch(
self,
goal_id: str,
*,
timeout: float | None = None,
) -> Iterator[NavigationFeedback | NavigationResult]:
request = navigation_pb2.WatchNavigationRequest(goal_id=goal_id)
try:
stream = self._session.navigation_stub.WatchNavigation(request, timeout=timeout)
for response in stream:
kind = response.WhichOneof("event")
if kind == "feedback":
yield _feedback_from_proto(response.feedback)
elif kind == "result":
yield _result_from_proto(response.result)
return
except grpc.RpcError as exc:
raise wrap_rpc_error(exc) from exc
[docs]
def handle(self, namespace: Optional[str] = None) -> "NavigationHandle":
"""Return a namespace-scoped navigation handle."""
resolved = self._session.resolve_namespace(namespace)
return NavigationHandle(self, resolved, self._session)
[docs]
class NavigationHandle:
"""Namespace-scoped handle for navigation behavior scripting."""
[docs]
def __init__(self, client: Navigation, namespace: str, session) -> None:
"""Bind to a client, resolved namespace, and spatial frames."""
from olo.spatial import Spatial
self._client = client
self.namespace = namespace
self._frames = Spatial(session).handle(namespace)
[docs]
def state(self, *, timeout: float = _DEFAULT_TIMEOUT_SEC) -> NavigationState:
"""Fetch a live capability snapshot for this robot."""
return self._client.state(self.namespace, timeout=timeout)
[docs]
def current_pose(self, *, timeout: float = _DEFAULT_TIMEOUT_SEC) -> Transform:
"""Return the stamped map-to-base transform via spatial TF lookup."""
base = self._frames.base_frame(timeout=timeout)
if not base:
raise OloFailedPrecondition(
f"No base frame detected for {self.namespace or '<global>'}"
)
return self._frames.lookup("map", base, timeout=timeout)
[docs]
def send_navigation_goal(
self,
target: Pose,
*,
frame: str = "map",
navigation_timeout: float = _DEFAULT_NAV_TIMEOUT_SEC,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> NavigationGoal:
"""Send a NavigateToPose goal without waiting for completion."""
return self._client.send_navigation_goal(
target,
robot_namespace=self.namespace,
frame=frame,
navigation_timeout=navigation_timeout,
timeout=timeout,
)
[docs]
def navigate_to(
self,
target: Pose,
*,
frame: str = "map",
navigation_timeout: float = _DEFAULT_NAV_TIMEOUT_SEC,
on_feedback: Callable[[NavigationFeedback], None] | None = None,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> NavigationResult:
"""Start navigation and wait for completion."""
return self._client.navigate_to(
target,
robot_namespace=self.namespace,
frame=frame,
navigation_timeout=navigation_timeout,
on_feedback=on_feedback,
timeout=timeout,
)
[docs]
def cancel_all(self, *, timeout: float = _DEFAULT_TIMEOUT_SEC) -> tuple[str, ...]:
"""Cancel all owned goals for this robot."""
return self._client.cancel_all(self.namespace, timeout=timeout)
[docs]
def get_map(
self,
*,
topic: str = "map",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> OccupancyGrid:
"""Fetch one occupancy-grid snapshot for this robot."""
return self._client.get_map(
self.namespace,
topic=topic,
timeout=timeout,
)
[docs]
def check_reachability(
self,
goal: Pose,
*,
frame: str = "map",
start: Pose | None = None,
use_start: bool = False,
planner_id: str = "",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> ReachabilityResult:
"""Ask Nav2 whether a goal pose is planner-reachable for this robot."""
return self._client.check_reachability(
goal,
robot_namespace=self.namespace,
frame=frame,
start=start,
use_start=use_start,
planner_id=planner_id,
timeout=timeout,
)
__all__ = [
"Activity",
"Availability",
"Navigation",
"Nav2ErrorCode",
"NavigationGoal",
"NavigationHandle",
"NavigationOutcome",
"NavigationResult",
"NavigationState",
"OccupancyGrid",
"ReachabilityResult",
]