"""Occupancy-grid helpers for navigation map inspection."""
from __future__ import annotations
import math
from dataclasses import dataclass
import numpy as np
from olo_protos.navigation.v1 import navigation_pb2
from olo._convert import timestamp_to_datetime
from olo.spatial import Pose
from olo.spatial._convert import pose_from_proto
_UNKNOWN = -1
_FREE = 0
[docs]
@dataclass(frozen=True, slots=True)
class OccupancyGrid:
"""Decoded occupancy grid with map-frame metadata."""
frame_id: str
resolution: float
width: int
height: int
origin: Pose
data: np.ndarray
stamp: float | None = None
def __post_init__(self) -> None:
if self.data.shape != (self.height, self.width):
raise ValueError(
f"Occupancy data shape {self.data.shape} does not match "
f"{self.width}x{self.height}"
)
if self.data.dtype != np.int8:
object.__setattr__(self, "data", np.ascontiguousarray(self.data, dtype=np.int8))
[docs]
def in_bounds(self, cell_x: int, cell_y: int) -> bool:
"""Return whether a grid cell lies inside the map."""
return 0 <= cell_x < self.width and 0 <= cell_y < self.height
[docs]
def value_at(self, cell_x: int, cell_y: int) -> int:
"""Return the occupancy value for a grid cell."""
if not self.in_bounds(cell_x, cell_y):
raise IndexError(f"Cell ({cell_x}, {cell_y}) is outside {self.width}x{self.height}")
return int(self.data[cell_y, cell_x])
[docs]
def world_to_cell(self, x: float, y: float) -> tuple[int, int]:
"""Convert a map-frame world point to grid indices."""
dx = float(x) - self.origin.position.x
dy = float(y) - self.origin.position.y
_, _, yaw = self.origin.orientation.to_rpy()
cos_yaw = math.cos(-yaw)
sin_yaw = math.sin(-yaw)
local_x = cos_yaw * dx - sin_yaw * dy
local_y = sin_yaw * dx + cos_yaw * dy
return (
int(math.floor(local_x / self.resolution)),
int(math.floor(local_y / self.resolution)),
)
[docs]
def cell_to_world(self, cell_x: int, cell_y: int) -> tuple[float, float]:
"""Return the map-frame centre of a grid cell."""
local_x = (float(cell_x) + 0.5) * self.resolution
local_y = (float(cell_y) + 0.5) * self.resolution
_, _, yaw = self.origin.orientation.to_rpy()
cos_yaw = math.cos(yaw)
sin_yaw = math.sin(yaw)
x = self.origin.position.x + cos_yaw * local_x - sin_yaw * local_y
y = self.origin.position.y + sin_yaw * local_x + cos_yaw * local_y
return x, y
[docs]
def pose_at(self, cell_x: int, cell_y: int, *, yaw: float = 0.0) -> Pose:
"""Return a map-frame pose at the centre of a grid cell."""
x, y = self.cell_to_world(cell_x, cell_y)
return Pose.from_xy_yaw(x, y, yaw)
[docs]
def is_unknown(self, cell_x: int, cell_y: int) -> bool:
"""Return whether a cell is unexplored."""
return self.value_at(cell_x, cell_y) < 0
[docs]
def is_free(self, cell_x: int, cell_y: int) -> bool:
"""Return whether a cell is known free space."""
return self.value_at(cell_x, cell_y) == _FREE
[docs]
def is_occupied(self, cell_x: int, cell_y: int) -> bool:
"""Return whether a cell is known occupied space."""
return self.value_at(cell_x, cell_y) > 0
def occupancy_grid_from_proto(message: navigation_pb2.OccupancyGrid) -> OccupancyGrid:
"""Decode a navigation OccupancyGrid protobuf."""
expected = int(message.width) * int(message.height)
raw = np.frombuffer(message.data, dtype=np.int8)
if raw.size != expected:
raise ValueError(
f"Occupancy data length {raw.size} does not match {message.width}x{message.height}"
)
stamp = (
timestamp_to_datetime(message.stamp).timestamp()
if message.HasField("stamp")
else None
)
return OccupancyGrid(
frame_id=message.frame_id,
stamp=stamp,
resolution=float(message.resolution),
width=int(message.width),
height=int(message.height),
origin=pose_from_proto(message.origin),
data=raw.reshape(int(message.height), int(message.width)),
)
__all__ = ["OccupancyGrid", "occupancy_grid_from_proto"]