from typing import TYPE_CHECKING
import matplotlib.pyplot as plt
import networkx as nx
import numpy as np
from matplotlib.patches import Polygon
from symaware.base import get_logger
if TYPE_CHECKING:
from typing import Literal
_Cell = tuple[int, int]
_Position = tuple[float, float]
Mode = Literal["center", "lower-left-corner", "default"]
[docs]
class GridMap:
"""
A 2D grid map that can be positioned, rotated, and scaled in continuous space.
The GridMap represents a discrete 2D grid that is embedded in a continuous coordinate system.
Each cell in the grid can be marked as occupied (True) or free (False). The grid can be:
- Positioned at any location using an offset
- Rotated by any angle
- Scaled using different cell sizes
Coordinate System and Geometry
-------------------------------
The grid uses a row-column indexing system where:
- **row (r)** corresponds to the x-axis in the local grid frame
- **col (c)** corresponds to the y-axis in the local grid frame
Grid Space (Discrete):
.. svgbob::
columns →
+---+---+---+---+
| | | | |
r +---+---+---+---+
o | | | | |
w +---+---+---+---+
s | | | | |
+---+---+---+---+
↓
Visual Guide - Tilting Your Head:
Normal view of the grid matrix:
[[0, 0, 1, 0], If you tilt your head 90° clockwise,
[0, 1, 0, 0], you can visualize how this maps to
[1, 0, 0, 1]] 2D space
After mental rotation, we get back the traditional Cartesian view:
.. svgbob::
↑ y
|
| +---+---+---+
| | | | 1 |
| +---+---+---+
| | 1 | | |
| +---+---+---+
| | | 1 | |
| +---+---+---+
| | | | 1 |
| +---+---+---+
|
+------------→ x
Args
----
grid_size:
Number of cells in the grid as (n_rows, n_cols)
cell_size:
Physical size of each cell as (width, height) in world units
offset:
Position of the grid origin (0,0) in world coordinates
rotation:
Rotation angle of the grid in radians, counter-clockwise
Attributes
----------
_grid: Boolean array representing occupied (True) and free (False) cells
_cell_size: Physical dimensions of each cell
_offset: World coordinates of the grid origin
_cos: Cosine of the rotation angle (cached for efficiency)
_sin: Sine of the rotation angle (cached for efficiency)
Examples
--------
Create a simple grid:
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap(grid_size=(10, 10), cell_size=(1.0, 1.0))
>>> grid.add_pos((5.5, 5.5)) # Mark cell containing position (5.5, 5.5) as occupied
Create a rotated grid:
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap(
... grid_size=(10, 10),
... cell_size=(2.0, 2.0),
... offset=(100.0, 100.0),
... rotation=np.pi/4 # 45 degrees
... )
See Also
--------
GridAbstraction : A more complex grid-based abstraction with MDP
"""
_LOGGER = get_logger(__name__, "GridMap")
def __init__(
self,
grid_size: "tuple[int, int]",
cell_size: "tuple[float, float]",
offset: "tuple[float, float] | None" = None,
rotation: float = 0.0,
):
"""
Initialize a GridMap with specified geometry.
Args
----
grid_size:
Number of cells as (n_rows, n_cols). Must be positive integers.
cell_size:
Physical size of each cell as (width, height). Must be positive.
offset:
World coordinates of grid origin. Defaults to (0.0, 0.0).
rotation:
Rotation angle in radians (counter-clockwise). Defaults to 0.0.
Raises
------
AssertionError
If grid_size or cell_size values are not positive.
"""
assert grid_size[0] > 0 and grid_size[1] > 0, "Grid size must be positive"
assert cell_size[0] > 0.0 and cell_size[1] > 0.0, "Cell size must be positive"
self._grid = np.zeros((grid_size), dtype=bool)
self._cell_size = cell_size
self._cos = np.cos(rotation)
self._sin = np.sin(rotation)
self._offset = offset if offset is not None else (0.0, 0.0)
[docs]
@classmethod
def from_bounds(cls, lb: "_Position", ub: "_Position", cell_size: "tuple[float, float]", rotation: float = 0.0):
"""
Create a GridMap from lower and upper bound positions from world coordinates.
This factory method automatically calculates the required grid size to cover
the rectangular region from lb to ub when rotated.
Geometry:
.. svgbob::
In world coordinates: In the grid's local frame (after rotation):
●
╱ ╲ ●----------------------● ub
╱ ╲ | |
╱ ╲ | |
lb ● ╲ | |
╲ ╲ lb ●----------------------●
╲ ╲
╲ ╲
╲ ● ub
╲ ╱
╲ ╱
╲ ╱
●
Grid size is computed to cover the rotated bounding box.
Args
----
lb:
Lower bound (bottom-left corner) in world coordinates
ub:
Upper bound (top-right corner) in world coordinates
cell_size:
Size of each grid cell as (width, height)
rotation:
Rotation angle in radians
Returns
-------
A new GridMap instance covering the specified bounds
Examples
--------
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap.from_bounds(
... lb=(0.0, 0.0),
... ub=(10.0, 20.0),
... cell_size=(1.0, 1.0)
... )
"""
cos = np.cos(-rotation)
sin = np.sin(-rotation)
corner_x = (ub[0] - lb[0]) * cos - (ub[1] - lb[1]) * sin
corner_y = (ub[0] - lb[0]) * sin + (ub[1] - lb[1]) * cos
grid_size_x = int(np.ceil(corner_x / cell_size[0]))
grid_size_y = int(np.ceil(corner_y / cell_size[1]))
return cls(grid_size=(grid_size_x, grid_size_y), cell_size=cell_size, offset=lb, rotation=rotation)
[docs]
@classmethod
def from_corner(
cls,
corner: "_Position",
map_size: "tuple[float, float]",
cell_size: "tuple[float, float]",
rotation: float = 0.0,
):
"""
Create a GridMap from a corner position (world coordinate) and map dimensions (world units).
This is useful when you know the physical size of the area you want to cover.
Geometry:
.. svgbob::
In world coordinates: In the grid's local frame (after rotation):
●
╱ ╲ ●----------------------●
height ╱ ╲ | |
╱ ╲ height | |
corner ● ╲ | |
╲ ╲ corner ●----------------------●
╲ ╲ width
╲ ╲
width ╲ ●
╲ ╱
╲ ╱
╲ ╱
●
Args
----
corner:
Position of the grid origin (lower-left corner) in world coordinates
map_size:
Physical dimensions of the map as (width, height)
cell_size:
Size of each grid cell as (width, height)
rotation:
Rotation angle in radians
Returns
-------
A new GridMap instance with the specified dimensions
Examples
--------
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap.from_corner(
... corner=(100.0, 100.0),
... map_size=(50.0, 50.0),
... cell_size=(1.0, 1.0),
... rotation=np.pi/4
... )
"""
offset = (corner[0], corner[1])
grid_size_x = int(np.ceil(map_size[0] / cell_size[0]))
grid_size_y = int(np.ceil(map_size[1] / cell_size[1]))
grid_size = (grid_size_x, grid_size_y)
return cls(grid_size=grid_size, cell_size=cell_size, offset=offset, rotation=rotation)
@property
def rotation_cos(self) -> float:
"""Cosine of the grid rotation angle."""
return self._cos
@property
def rotation_sin(self) -> float:
"""Sine of the grid rotation angle."""
return self._sin
@property
def cell_size(self) -> "tuple[float, float]":
"""Physical size of each cell as (width, height)."""
return self._cell_size
@property
def cell_width(self) -> float:
"""Width of each grid cell."""
return self._cell_size[0]
@property
def cell_height(self) -> float:
"""Height of each grid cell."""
return self._cell_size[1]
@property
def width(self) -> float:
"""Total width of the grid in world coordinates."""
return self._grid.shape[0] * self._cell_size[0]
@property
def height(self) -> float:
"""Total height of the grid in world coordinates."""
return self._grid.shape[1] * self._cell_size[1]
@property
def rows(self) -> int:
"""Number of rows in the grid."""
return self._grid.shape[0]
@property
def cols(self) -> int:
"""Number of columns in the grid."""
return self._grid.shape[1]
@property
def grid(self) -> "np.ndarray":
"""The underlying grid array (boolean matrix)."""
return self._grid
@property
def all_pos(self) -> "list[_Position]":
"""List of all occupied cell positions."""
return self.get_all_pos(mode="default")
@property
def all_centers(self) -> "list[_Position]":
"""List of all occupied cell center positions."""
return self.get_all_pos(mode="center")
@property
def num_occupied(self) -> int:
"""Number of occupied cells in the grid."""
return np.count_nonzero(self._grid)
[docs]
def get_all_pos(self, mode: "Mode" = "default") -> "list[_Position]":
positions = []
for r in range(self._grid.shape[0]):
for c in range(self._grid.shape[1]):
if self.is_occupied(r, c):
positions.append(self.cell_to_pos((r, c), mode=mode))
return positions
[docs]
def is_occupied(self, row: int, col: int) -> bool:
"""Check if a cell in the grid is occupied."""
if not (0 <= row < self._grid.shape[0] and 0 <= col < self._grid.shape[1]):
return False
return self._grid[row, col]
def __getitem__(self, key: "tuple[int, int]") -> bool:
row, col = key
return self._grid[row, col]
[docs]
def set_cell(self, row: int, col: int, value: bool):
assert isinstance(value, bool), "Value must be a boolean"
self._grid[row, col] = value
[docs]
def add_cell(self, row: int, col: int):
"""Activate a cell in the grid at the specified row and column."""
self.set_cell(row, col, True)
[docs]
def remove_cell(self, row: int, col: int):
"""Deactivate a cell in the grid at the specified row and column."""
self.set_cell(row, col, False)
[docs]
def set_cell_range(self, row_lb: int, row_ub: int, col_lb: int, col_ub: int, value: bool):
"""Set a range of cells in the grid."""
self._grid[row_lb:row_ub, col_lb:col_ub] = value
[docs]
def add_cell_range(self, row_lb: int, row_ub: int, col_lb: int, col_ub: int):
"""Activate a range of cells in the grid."""
self.set_cell_range(row_lb, row_ub, col_lb, col_ub, True)
[docs]
def remove_cell_range(self, row_lb: int, row_ub: int, col_lb: int, col_ub: int):
"""Deactivate a range of cells in the grid."""
self.set_cell_range(row_lb, row_ub, col_lb, col_ub, False)
[docs]
def fill(self):
"""Fill the entire grid with the specified boolean value."""
self._grid[:, :] = True
[docs]
def clear(self):
"""Clear the grid by setting all cells to False."""
self._grid[:, :] = False
[docs]
def set_pos(self, pos: "_Position", value: bool):
"""
Set a cell in the grid based on a world position.
Args
----
pos: Continuous position as (x, y) coordinates
value: Boolean value to set the cell to
"""
row, col = self.pos_to_cell(pos)
self.set_cell(row, col, value)
[docs]
def add_pos(self, pos: "_Position"):
"""
Add a cell to the grid based on a world position.
Args
----
pos: Continuous position as (x, y) coordinates
"""
self.set_pos(pos, True)
[docs]
def remove_pos(self, pos: "_Position"):
"""
Remove a cell from the grid based on a woorld position.
Args
----
pos: Continuous position as (x, y) coordinates
"""
self.set_pos(pos, False)
[docs]
def set_pos_range(self, lb: "_Position", ub: "_Position", value: bool):
"""
Set a range of cells in the grid based on world coordinate bounds.
Args
----
lb: Lower bound for position (x, y)
ub: Upper bound for position (x, y)
value: Boolean value to set the cells to
"""
cell_lb = self.pos_to_cell(lb)
cell_ub = self.pos_to_cell(ub)
self.set_cell_range(cell_lb[0], cell_ub[0] + 1, cell_lb[1], cell_ub[1] + 1, value)
[docs]
def add_pos_range(self, lb: "_Position", ub: "_Position"):
"""
Add a range of cells to the grid based on world coordinate bounds.
Args
----
lb: Lower bound for position (x, y)
ub: Upper bound for position (x, y)
"""
self.set_pos_range(lb, ub, True)
[docs]
def remove_pos_range(self, lb: "_Position", ub: "_Position"):
"""
Remove a range of cells from the grid based on world coordinate bounds.
Args
----
lb: Lower bound for position (x, y)
ub: Upper bound for position (x, y)
"""
self.set_pos_range(lb, ub, False)
[docs]
def cell_to_pos(self, cell: "_Cell", mode: "Mode" = "default") -> "_Position":
"""
Convert grid cell coordinates to continuous world position.
This method applies the full geometric transformation:
1. Scale: Multiply cell indices by cell_size
2. Rotate: Apply rotation matrix
3. Translate: Add offset
Transformation Diagram:
Grid Space → Scale back → Rotate → Offset → World Space
Args
----
cell:
Grid cell coordinates as (row, col)
mode:
Position within the cell to return:
- "center": Center of the cell (default)
- "lower-left-corner": Lower-left corner of the cell
Returns
-------
World coordinates (x, y) of the cell
Examples
--------
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap((10, 10), (1.0, 1.0), offset=(0, 0))
>>> grid.cell_to_pos((5, 5)) # Center of cell (5,5)
(5.5, 5.5)
>>> grid.cell_to_pos((5, 5), mode="lower-left-corner")
(5.0, 5.0)
"""
row, col = cell
x_rot = row * self._cell_size[0] + (self._cell_size[0] / 2 if mode != "lower-left-corner" else 0)
y_rot = col * self._cell_size[1] + (self._cell_size[1] / 2 if mode != "lower-left-corner" else 0)
x_correct = x_rot * self._cos - y_rot * self._sin
y_correct = x_rot * self._sin + y_rot * self._cos
x = x_correct + self._offset[0]
y = y_correct + self._offset[1]
return (x, y)
[docs]
def pos_to_cell(self, pos: "_Position") -> "_Cell":
"""
Convert continuous world position to grid cell coordinates.
This method applies the inverse geometric transformation:
1. Translate: Subtract offset
2. Rotate: Apply inverse rotation matrix (transpose)
3. Scale: Divide by cell_size and floor
Inverse Transformation Diagram:
World Space → Remove offset → Rotate → Scale → Grid Space
Args
----
pos:
World coordinates as (x, y)
Returns
-------
Grid cell coordinates as (row, col)
Raises
------
AssertionError
If the position is outside the grid bounds
Examples
--------
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap((10, 10), (1.0, 1.0), offset=(0, 0))
>>> grid.pos_to_cell((5.7, 3.2))
(5, 3)
"""
x, y = pos
x_correct, y_correct = x - self._offset[0], y - self._offset[1]
x_rot = x_correct * self._cos + y_correct * self._sin
y_rot = -x_correct * self._sin + y_correct * self._cos
row = int(x_rot // self._cell_size[0])
col = int(y_rot // self._cell_size[1])
return (row, col)
[docs]
def plot_grid(self, ax: "plt.Axes | None" = None) -> "tuple[plt.Figure, plt.Axes]":
"""
Plot the grid in its native discrete space (row-column view).
This shows the grid as a matrix where:
- X-axis represents rows
- Y-axis represents columns
- Black cells are occupied (True)
- White cells are free (False)
Grid Visualization:
.. svgbob::
cols →
rows┌─────────┐
↓ │░░ ░ ░ │ ← Black = occupied
│ ░ ░ │ ← White = free
│ ░ ░ │
└─────────┘
Returns
-------
Matplotlib figure and axes objects
"""
fig, ax = plt.subplots() if ax is None else (ax.get_figure(), ax)
ax.imshow(self._grid.T, origin="lower", cmap="Greys", interpolation="nearest", vmin=0, vmax=1)
ax.set_xlabel("Rows (X)")
ax.set_ylabel("Columns (Y)")
ax.set_title("Grid Map")
return fig, ax
[docs]
def plot_grid_projection(
self,
points: "_Position | list[_Position] | None" = None,
point_color: "str | list[str]" = "green",
point_marker: "str" = "o",
point_size: "int" = 50,
ax: "plt.Axes | None" = None,
) -> "tuple[plt.Figure, plt.Axes]":
"""
Plot the grid projected into continuous world space.
This visualization shows:
- Occupied cells as red squares in their world positions
- Grid boundary as a blue polygon
- The effect of rotation and offset on the grid layout
- Optional points in world coordinates
Args
----
points: Optional point or list of points in world coordinates (x, y) to draw on the grid
point_color: Color for the points (default: "green")
point_marker: Marker style for the points (default: "o" for circle)
point_size: Size of the point markers (default: 50)
Returns
-------
Matplotlib figure and axes objects, or None if no occupied cells
Notes
-----
The marker size is proportional to cell_size to give an accurate
representation of cell dimensions in world space.
"""
occupied_positions = []
for row in range(self._grid.shape[0]):
for col in range(self._grid.shape[1]):
if self.is_occupied(row, col):
pos = self.cell_to_pos((row, col))
occupied_positions.append(pos)
x_vals = [pos[0] for pos in occupied_positions]
y_vals = [pos[1] for pos in occupied_positions]
fig, ax = plt.subplots() if ax is None else (ax.get_figure(), ax)
# Draw the grid boundary as a rectangle
# Get the four corners of the grid in real space
corner_00 = self.cell_to_pos((0, 0), mode="lower-left-corner")
corner_10 = self.cell_to_pos((self._grid.shape[0], 0), mode="lower-left-corner")
corner_11 = self.cell_to_pos((self._grid.shape[0], self._grid.shape[1]), mode="lower-left-corner")
corner_01 = self.cell_to_pos((0, self._grid.shape[1]), mode="lower-left-corner")
min_x = min(corner_00[0], corner_10[0], corner_11[0], corner_01[0])
max_x = max(corner_00[0], corner_10[0], corner_11[0], corner_01[0])
min_y = min(corner_00[1], corner_10[1], corner_11[1], corner_01[1])
max_y = max(corner_00[1], corner_10[1], corner_11[1], corner_01[1])
# Create a polygon for the grid boundary
grid_corners = [corner_00, corner_10, corner_11, corner_01]
grid_polygon = Polygon(grid_corners, fill=False, edgecolor="blue", linewidth=2, label="Grid Boundary")
ax.add_patch(grid_polygon)
ax.scatter(x_vals, y_vals, c="red", marker="x")
# Draw user-provided points if any
if points is not None:
if not isinstance(points, list):
points = [points]
if len(points) > 0:
point_x_vals = [p[0] for p in points]
point_y_vals = [p[1] for p in points]
colors = point_color if isinstance(point_color, list) else [point_color] * len(points)
ax.scatter(
point_x_vals, point_y_vals, c=colors, marker=point_marker, s=point_size, zorder=5, label="Points"
)
ax.set_xlim(min_x - self._cell_size[0], max_x + self._cell_size[0])
ax.set_ylim(min_y - self._cell_size[1], max_y + self._cell_size[1])
ax.set_xlabel("X Position")
ax.set_ylabel("Y Position")
ax.set_title("Occupied Positions")
ax.legend()
return fig, ax
[docs]
def compute_paths_pos(
self,
start: "_Position",
targets: "_Position | list[_Position]",
use_8_connectivity: bool = True,
) -> "list[list[_Position]]":
"""
Compute all possible paths from a starting position to a target position.
This is a convenience wrapper around compute_paths that accepts positions
instead of cell indices.
Args
----
start_pos:
Starting position as (x, y) coordinates
target_pos:
Target position as (x, y) coordinates
use_8_connectivity:
If True, uses 8-connectivity (includes diagonals). If False, uses 4-connectivity.
Default is True (King's move pattern).
Returns
-------
List of paths, where each path is a list of positions (x, y) from start to target.
Returns empty list if no path exists or if start/target are not in occupied cells.
Examples
--------
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap((10, 10), (1.0, 1.0))
>>> grid.add_cell_range(0, 10, 0, 10)
>>> paths = grid.compute_paths_pos((0.5, 0.5), (5.5, 5.5))
>>> print(f"Found {len(paths)} paths")
Found 1 paths
"""
if not isinstance(targets, list):
targets = [targets]
start_cell = self.pos_to_cell(start)
target_cells = [self.pos_to_cell(target) for target in targets]
cell_paths = self.compute_paths(start_cell, target_cells, use_8_connectivity)
# Convert cell paths back to position paths
pos_paths = []
for cell_path in cell_paths:
pos_paths.append([self.cell_to_pos(cell) for cell in cell_path])
return pos_paths
[docs]
def compute_paths(
self,
start: "_Cell",
targets: "list[_Cell] | _Cell",
use_8_connectivity: bool = True,
) -> "dict[_Cell, list[list[_Cell]]]":
"""
Compute all shortest paths from a starting cell to a list of target cells.
This method treats the occupied cells in the grid as nodes in a graph, where adjacent
cells are connected. It uses networkx to find all simple paths between start and each target.
Target nodes are guaranteed to have no edges connecting them, even if they are adjacent.
Graph Structure:
.. svgbob::
For 8-connectivity:
↖ ↑ ↗
← ● →
↙ ↓ ↘
For 4-connectivity:
↑
← ● →
↓
Args
----
start:
Starting cell as (row, col) indices
targets:
Target cell or list of target cells as (row, col) indices
use_8_connectivity:
If True, uses 8-connectivity (includes diagonals). If False, uses 4-connectivity.
Returns
-------
List of paths, where each path is a list of cells from start to target.
Examples
--------
>>> from symaware.simulators.carla import GridMap
>>> grid = GridMap((10, 10), (1.0, 1.0))
>>> grid.add_cell_range(0, 10, 0, 10)
>>> paths = grid.compute_paths((0, 0), [(5, 5), (7, 7)])
Notes
-----
- Only occupied cells are considered as valid nodes in the graph
- Target nodes will never have edges between them, ensuring path independence
"""
# Normalize targets to list
if not isinstance(targets, list):
targets = [targets]
# Verify that start cell is occupied
if not self.is_occupied(*start):
self._LOGGER.warning("Start cell %s is not occupied", start)
return []
# Verify that all target cells are occupied
targets = list(filter(lambda t: self.is_occupied(*t), targets))
if len(targets) == 0:
self._LOGGER.warning("No target cells are occupied")
return []
# Build graph from occupied cells
G = nx.Graph()
# Define neighbor offsets based on connectivity
if use_8_connectivity:
# 8-connectivity (King's move): includes diagonals
neighbors = ((-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1))
else:
# 4-connectivity: only orthogonal neighbors
neighbors = ((-1, 0), (0, -1), (0, 1), (1, 0))
# Convert targets list to set for fast lookup
target_set = set(targets)
# Add nodes and edges for all occupied cells
for row in range(self._grid.shape[0]):
for col in range(self._grid.shape[1]):
if not self.is_occupied(row, col):
continue
current_cell = (row, col)
# Add node for this cell
G.add_node(current_cell)
# Check all neighbors and add edges
for dr, dc in neighbors:
neighbor_row = row + dr
neighbor_col = col + dc
# Check if neighbor is within bounds and occupied
if (
0 <= neighbor_row < self._grid.shape[0]
and 0 <= neighbor_col < self._grid.shape[1]
and self.is_occupied(neighbor_row, neighbor_col)
):
neighbor_cell = (neighbor_row, neighbor_col)
# Skip edges between target nodes
if current_cell in target_set and neighbor_cell in target_set:
continue
weight = 1 if (dr == 0 or dc == 0) else np.sqrt(2)
# Add edge between current cell and neighbor
G.add_edge(current_cell, neighbor_cell, weight=weight)
# Find all simple paths for each target
return tuple(
nx.shortest_path(G, start, target, weight="weight") for target in targets if nx.has_path(G, start, target)
)
[docs]
def visualize_paths(
self,
paths: "list[list[_Cell]]",
start: "_Cell | None" = None,
targets: "list[_Cell] | None" = None,
) -> "tuple[plt.Figure, plt.Axes]":
"""
Visualize the computed paths on the grid.
Args
----
paths:
List of paths, where each path is a list of cells (row, col)
start:
Starting cell to highlight. If None, uses first cell of first path.
targets:
Target cells to highlight. If None, infers from paths dict keys or last cells.
Returns
-------
Matplotlib figure and axes objects
"""
fig, ax = plt.subplots(figsize=(10, 10))
# Infer targets from last cells if not provided
targets = {path[-1] for path in paths if len(path) > 0} if targets is None else targets
# Plot the base grid
grid_display = np.zeros(self._grid.shape[:2], dtype=bool)
for row in range(self._grid.shape[0]):
for col in range(self._grid.shape[1]):
if self.is_occupied(row, col):
grid_display[row, col] = True
ax.imshow(grid_display.T, origin="lower", cmap="Greys", alpha=0.3)
# Plot each path with a different color
colors = plt.cm.rainbow(np.linspace(0, 1, len(paths)))
for path_idx, path in enumerate(paths):
if len(path) < 2:
continue
path_array = np.array(path)
ax.plot(
path_array[:, 0],
path_array[:, 1],
"-o",
color=colors[path_idx],
alpha=0.6,
linewidth=2,
markersize=4,
label=f"Path {path_idx + 1} (len={len(path)})",
)
# Highlight start
if start is None and len(paths) > 0 and len(paths[0]) > 0:
start = paths[0][0]
if start is not None:
ax.plot(start[0], start[1], "go", markersize=15, label="Start", zorder=10)
# Highlight all targets
if targets is not None:
for i, target in enumerate(targets):
ax.plot(
target[0],
target[1],
"r*",
markersize=20,
label=f"Target {i+1}" if len(targets) > 1 else "Target",
zorder=10,
)
ax.set_xlabel("Row")
ax.set_ylabel("Column")
ax.set_title(f"Path Visualization ({len(paths)} paths found)")
ax.legend(bbox_to_anchor=(1.05, 1), loc="upper left")
ax.grid(True, alpha=0.3)
plt.tight_layout()
return fig, ax
[docs]
class WaypointGridMap(GridMap):
def __init__(self, grid_size, cell_size, offset=None, rotation=0):
super().__init__(grid_size, cell_size, offset, rotation)
self._grid = np.full((*self._grid.shape, 2), np.nan, dtype=float)
[docs]
def cell_to_pos(self, cell: "_Cell", mode: "Mode" = "default") -> "_Position":
if mode != "default":
return super().cell_to_pos(cell, mode)
return tuple(self._grid[cell[0], cell[1]])
[docs]
def clear(self):
"""Clear the grid by setting all cells to NaN."""
self._grid[:, :] = np.nan
[docs]
def fill(self):
raise NotImplementedError("Filling the entire grid is not supported in WaypointGridMap.")
[docs]
def add_pos(self, pos: "_Position"):
self.set_pos(pos, pos)
[docs]
def remove_pos(self, pos: "_Position"):
self.set_pos(pos, (np.nan, np.nan))
[docs]
def set_cell(self, row: int, col: int, value: "_Position | None"):
assert len(value) == 2, "Value must be a tuple of (x, y) coordinates"
self._grid[row, col, 0] = value[0]
self._grid[row, col, 1] = value[1]
[docs]
def add_cell(self, row: int, col: int, pos: "_Position | None" = None):
if pos is None:
pos = super().cell_to_pos((row, col), mode="center")
self.set_cell(row, col, pos)
[docs]
def remove_cell(self, row: int, col: int):
self.set_cell(row, col, (np.nan, np.nan))
[docs]
def set_cell_range(self, row_lb: int, row_ub: int, col_lb: int, col_ub: int, value: bool):
raise NotImplementedError("Setting a range of cells is not supported in WaypointGridMap.")
[docs]
def is_occupied(self, row: int, col: int) -> bool:
if not (0 <= row < self._grid.shape[0] and 0 <= col < self._grid.shape[1]):
return False
return not np.isnan(self._grid[row, col, 0]) and not np.isnan(self._grid[row, col, 1])
@property
def num_occupied(self) -> int:
return np.count_nonzero(~np.isnan(self._grid[:, :, 0]))
[docs]
def plot_grid(self, ax: "plt.Axes | None" = None) -> "tuple[plt.Figure, plt.Axes]":
"""
Plot the grid in its native discrete space (row-column view).
This shows the grid as a matrix where:
- X-axis represents rows
- Y-axis represents columns
- Black cells are occupied (True)
- White cells are free (False)
Grid Visualization:
.. svgbob::
cols →
rows┌─────────┐
↓ │░░ ░ ░ │ ← Black = occupied
│ ░ ░ │ ← White = free
│ ░ ░ │
└─────────┘
Returns
-------
Matplotlib figure and axes objects
"""
fig, ax = plt.subplots() if ax is None else (ax.get_figure(), ax)
# Create a boolean mask for occupied cells
occupied_mask = ~np.isnan(self._grid[:, :, 0])
ax.imshow(occupied_mask.T, origin="lower", cmap="Greys", interpolation="nearest", vmin=0, vmax=1)
ax.set_xlabel("Rows (X)")
ax.set_ylabel("Columns (Y)")
ax.set_title("Grid Map")
return fig, ax
[docs]
class PathsGridMap(WaypointGridMap):
def __init__(self, grid_size, cell_size, paths: "list[tuple[_Position]] | None" = None, offset=None, rotation=0):
super().__init__(grid_size, cell_size, offset, rotation)
self._edges: "dict[tuple[_Cell, int], _Cell]" = {}
self._num_paths = 0
self._terminating_cells: "list[_Cell]" = []
self.set_paths(paths if paths is not None else [])
@property
def num_paths(self):
return self._num_paths
@property
def terminating_cells(self) -> "list[_Cell]":
return self._terminating_cells
@property
def edges(self) -> "dict[tuple[_Cell, int], _Cell]":
return self._edges
[docs]
def set_paths(self, paths: "list[tuple[_Position]]", additional_range: int = 0):
self._terminating_cells.clear()
self._edges.clear()
self.clear()
self._num_paths = len(paths)
additional_cells: "dict[tuple[int, int], _Position]" = {}
for path_idx, path in enumerate(paths):
connections: set[_Cell] = set()
for i in range(len(path) - 1):
cell_from = self.pos_to_cell(path[i])
cell_to = self.pos_to_cell(path[i + 1])
# Avoid loops by ensuring each cell can only be reached once across each path.
if cell_to in connections:
continue
self._edges[(cell_from, path_idx)] = cell_to
self.add_cell(cell_from[0], cell_from[1], path[i])
connections.add(cell_to)
for x_off in range(-additional_range, additional_range + 1):
for y_off in range(-additional_range, additional_range + 1):
if x_off == 0 and y_off == 0:
continue
new_pos = (path[i][0] + x_off * self.cell_width, path[i][1] + y_off * self.cell_height)
additional_cells[(cell_from[0] + x_off, cell_from[1] + y_off)] = new_pos
# Add last cell
last_cell = self.pos_to_cell(path[-1])
self.add_cell(last_cell[0], last_cell[1], path[-1])
self._terminating_cells.append(last_cell)
for cell, pos in additional_cells.items():
if not self.is_occupied(cell[0], cell[1]):
self.add_cell(cell[0], cell[1], pos)
[docs]
@classmethod
def from_grid_map(
cls,
grid_map: GridMap,
start: "_Position",
targets: "list[_Position]",
):
return cls(
paths=grid_map.compute_paths_pos(start, targets),
grid_size=(grid_map.rows, grid_map.cols),
cell_size=grid_map.cell_size,
offset=grid_map._offset,
rotation=np.arctan2(grid_map._sin, grid_map._cos),
)