1085 lines
47 KiB
Python
1085 lines
47 KiB
Python
from __future__ import annotations
|
||
|
||
import heapq
|
||
import logging
|
||
import math
|
||
from dataclasses import dataclass, field
|
||
from typing import Any, Dict, Iterable, List, Optional, Tuple
|
||
|
||
import numpy as np
|
||
|
||
_log = logging.getLogger("SLAM_NavRuntime")
|
||
|
||
|
||
def _safe_float(v: Any, default: float) -> float:
|
||
try:
|
||
return float(v)
|
||
except Exception:
|
||
return float(default)
|
||
|
||
|
||
def _safe_int(v: Any, default: int) -> int:
|
||
try:
|
||
return int(v)
|
||
except Exception:
|
||
return int(default)
|
||
|
||
|
||
def _as_bool(v: Any, default: bool) -> bool:
|
||
if v is None:
|
||
return default
|
||
if isinstance(v, bool):
|
||
return v
|
||
if isinstance(v, (int, float)):
|
||
return bool(v)
|
||
return str(v).strip().lower() in ("1", "true", "yes", "on")
|
||
|
||
|
||
# Disc-offset cache for `_inflate_binary`. Keyed on radius_cells; the disc
|
||
# of `(dy, dx)` offsets within `r` is identical across calls for the same
|
||
# radius. With `inflation_radius_m / res` typically rounding to 2-3 cells,
|
||
# this cache holds a tiny number of entries and pays back its allocation
|
||
# every tick after the first.
|
||
_INFLATE_OFFSETS_CACHE: Dict[int, List[Tuple[int, int]]] = {}
|
||
|
||
|
||
def _world_to_grid_strict(
|
||
x: float, y: float, origin: np.ndarray, res: float, w: int, h: int,
|
||
) -> Optional[Tuple[int, int]]:
|
||
"""Convert world (x, y) to grid (gx, gy); returns None if outside [0, w) x [0, h).
|
||
Centralised so the bounds-check semantics stay identical across world_to_grid,
|
||
A* to_grid, and any future cell-locator.
|
||
"""
|
||
gx = int(math.floor((float(x) - float(origin[0])) / float(res)))
|
||
gy = int(math.floor((float(y) - float(origin[1])) / float(res)))
|
||
if gx < 0 or gy < 0 or gx >= int(w) or gy >= int(h):
|
||
return None
|
||
return gx, gy
|
||
|
||
|
||
def _inflate_offsets(radius_cells: int) -> List[Tuple[int, int]]:
|
||
cached = _INFLATE_OFFSETS_CACHE.get(int(radius_cells))
|
||
if cached is not None:
|
||
return cached
|
||
rr = int(radius_cells) * int(radius_cells)
|
||
offsets: List[Tuple[int, int]] = []
|
||
for dy in range(-radius_cells, radius_cells + 1):
|
||
for dx in range(-radius_cells, radius_cells + 1):
|
||
if (dy * dy + dx * dx) <= rr:
|
||
offsets.append((dy, dx))
|
||
_INFLATE_OFFSETS_CACHE[int(radius_cells)] = offsets
|
||
return offsets
|
||
|
||
|
||
def _inflate_binary(mask: np.ndarray, radius_cells: int) -> np.ndarray:
|
||
if radius_cells <= 0:
|
||
return mask.copy()
|
||
h, w = mask.shape
|
||
out = mask.copy()
|
||
for dy, dx in _inflate_offsets(int(radius_cells)):
|
||
ys = max(0, -dy)
|
||
ye = min(h, h - dy)
|
||
xs = max(0, -dx)
|
||
xe = min(w, w - dx)
|
||
yd = max(0, dy)
|
||
xd = max(0, dx)
|
||
out[yd : yd + (ye - ys), xd : xd + (xe - xs)] |= mask[ys:ye, xs:xe]
|
||
return out
|
||
|
||
|
||
@dataclass
|
||
class LiveCostmapConfig:
|
||
enabled: bool = True
|
||
resolution_m: float = 0.10
|
||
z_min_m: float = -0.40
|
||
z_max_m: float = 1.20
|
||
padding_m: float = 0.80
|
||
inflation_radius_m: float = 0.25
|
||
dynamic_decay_sec: float = 1.5
|
||
dynamic_min_hits: int = 2
|
||
blocked_cost: int = 220
|
||
max_width_cells: int = 900
|
||
max_height_cells: int = 900
|
||
|
||
@staticmethod
|
||
def from_dict(d: Dict[str, Any] | None) -> "LiveCostmapConfig":
|
||
src = d or {}
|
||
return LiveCostmapConfig(
|
||
enabled=_as_bool(src.get("enabled", True), True),
|
||
resolution_m=max(0.02, _safe_float(src.get("resolution_m", 0.10), 0.10)),
|
||
z_min_m=_safe_float(src.get("z_min_m", -0.40), -0.40),
|
||
z_max_m=_safe_float(src.get("z_max_m", 1.20), 1.20),
|
||
padding_m=max(0.0, _safe_float(src.get("padding_m", 0.80), 0.80)),
|
||
inflation_radius_m=max(0.0, _safe_float(src.get("inflation_radius_m", 0.25), 0.25)),
|
||
dynamic_decay_sec=max(0.2, _safe_float(src.get("dynamic_decay_sec", 1.5), 1.5)),
|
||
dynamic_min_hits=max(1, _safe_int(src.get("dynamic_min_hits", 2), 2)),
|
||
blocked_cost=int(np.clip(_safe_int(src.get("blocked_cost", 220), 220), 100, 255)),
|
||
max_width_cells=max(100, _safe_int(src.get("max_width_cells", 900), 900)),
|
||
max_height_cells=max(100, _safe_int(src.get("max_height_cells", 900), 900)),
|
||
)
|
||
|
||
def patched(self, patch: Dict[str, Any] | None) -> "LiveCostmapConfig":
|
||
src = patch or {}
|
||
return LiveCostmapConfig.from_dict(
|
||
{
|
||
"enabled": src.get("enabled", self.enabled),
|
||
"resolution_m": src.get("resolution_m", self.resolution_m),
|
||
"z_min_m": src.get("z_min_m", self.z_min_m),
|
||
"z_max_m": src.get("z_max_m", self.z_max_m),
|
||
"padding_m": src.get("padding_m", self.padding_m),
|
||
"inflation_radius_m": src.get("inflation_radius_m", self.inflation_radius_m),
|
||
"dynamic_decay_sec": src.get("dynamic_decay_sec", self.dynamic_decay_sec),
|
||
"dynamic_min_hits": src.get("dynamic_min_hits", self.dynamic_min_hits),
|
||
"blocked_cost": src.get("blocked_cost", self.blocked_cost),
|
||
"max_width_cells": src.get("max_width_cells", self.max_width_cells),
|
||
"max_height_cells": src.get("max_height_cells", self.max_height_cells),
|
||
}
|
||
)
|
||
|
||
|
||
class LiveCostmapRuntime:
|
||
"""
|
||
Maintains real-time static + dynamic + inflated obstacle layers.
|
||
Static layer comes from SLAM stable points; dynamic layer comes from live points.
|
||
"""
|
||
|
||
def __init__(self, cfg: LiveCostmapConfig):
|
||
self.cfg = cfg
|
||
self._dynamic_cells: Dict[Tuple[int, int], Tuple[int, float, float, float]] = {}
|
||
self._grid: Optional[Dict[str, Any]] = None
|
||
|
||
def reset(self) -> None:
|
||
self._dynamic_cells.clear()
|
||
self._grid = None
|
||
|
||
def _clip_nav_band(self, points: Optional[np.ndarray]) -> np.ndarray:
|
||
if points is None:
|
||
return np.zeros((0, 3), dtype=np.float32)
|
||
pts = np.asarray(points, dtype=np.float32)
|
||
if pts.ndim != 2 or pts.shape[1] < 3 or len(pts) == 0:
|
||
return np.zeros((0, 3), dtype=np.float32)
|
||
zmin = float(self.cfg.z_min_m)
|
||
zmax = float(self.cfg.z_max_m)
|
||
m = (pts[:, 2] >= zmin) & (pts[:, 2] <= zmax)
|
||
return pts[m, :3]
|
||
|
||
@staticmethod
|
||
def _grid_indices(xy: np.ndarray, min_xy: np.ndarray, res: float, w: int, h: int) -> Tuple[np.ndarray, np.ndarray]:
|
||
gx = np.floor((xy[:, 0] - min_xy[0]) / res).astype(np.int32)
|
||
gy = np.floor((xy[:, 1] - min_xy[1]) / res).astype(np.int32)
|
||
gx = np.clip(gx, 0, w - 1)
|
||
gy = np.clip(gy, 0, h - 1)
|
||
return gx, gy
|
||
|
||
def world_to_grid(self, x: float, y: float) -> Optional[Tuple[int, int]]:
|
||
if self._grid is None:
|
||
return None
|
||
origin = np.asarray(self._grid["origin_xy"], dtype=np.float64)
|
||
res = float(self._grid["resolution_m"])
|
||
shape = tuple(self._grid["shape_hw"])
|
||
h, w = int(shape[0]), int(shape[1])
|
||
return _world_to_grid_strict(x, y, origin, res, w, h)
|
||
|
||
def cost_at_world(self, x: float, y: float) -> int:
|
||
if self._grid is None:
|
||
return 255
|
||
idx = self.world_to_grid(x, y)
|
||
if idx is None:
|
||
return 255
|
||
gx, gy = idx
|
||
cost = np.asarray(self._grid["costmap"], dtype=np.uint8)
|
||
return int(cost[gy, gx])
|
||
|
||
def occupied_near(self, x: float, y: float, radius_m: float, cost_thresh: Optional[int] = None) -> bool:
|
||
if self._grid is None:
|
||
return True
|
||
thresh = int(self.cfg.blocked_cost if cost_thresh is None else cost_thresh)
|
||
idx = self.world_to_grid(x, y)
|
||
if idx is None:
|
||
return True
|
||
gx, gy = idx
|
||
cost = np.asarray(self._grid["costmap"], dtype=np.uint8)
|
||
h, w = cost.shape
|
||
rc = int(math.ceil(max(0.0, float(radius_m)) / float(self._grid["resolution_m"])))
|
||
ys = max(0, gy - rc)
|
||
ye = min(h, gy + rc + 1)
|
||
xs = max(0, gx - rc)
|
||
xe = min(w, gx + rc + 1)
|
||
return bool(np.any(cost[ys:ye, xs:xe] >= thresh))
|
||
|
||
def _compute_bounds(self, static_pts: np.ndarray, live_pts: np.ndarray) -> Optional[Tuple[np.ndarray, np.ndarray]]:
|
||
has_static = len(static_pts) > 0
|
||
has_live = len(live_pts) > 0
|
||
if not has_static and not has_live:
|
||
return None
|
||
if has_static and has_live:
|
||
all_xy = np.vstack((static_pts[:, :2], live_pts[:, :2]))
|
||
elif has_static:
|
||
all_xy = static_pts[:, :2]
|
||
else:
|
||
all_xy = live_pts[:, :2]
|
||
pad = float(self.cfg.padding_m)
|
||
mn = all_xy.min(axis=0) - pad
|
||
mx = all_xy.max(axis=0) + pad
|
||
return mn.astype(np.float64), mx.astype(np.float64)
|
||
|
||
def _clamp_grid_size(self, mn: np.ndarray, mx: np.ndarray, res: float) -> Tuple[np.ndarray, np.ndarray, float, int, int]:
|
||
span = np.maximum(mx - mn, res)
|
||
w = int(np.ceil(span[0] / res)) + 1
|
||
h = int(np.ceil(span[1] / res)) + 1
|
||
res_out = float(res)
|
||
if w > int(self.cfg.max_width_cells) or h > int(self.cfg.max_height_cells):
|
||
sx = float(w) / float(self.cfg.max_width_cells)
|
||
sy = float(h) / float(self.cfg.max_height_cells)
|
||
scale = max(sx, sy)
|
||
res_out = res_out * scale
|
||
span = np.maximum(mx - mn, res_out)
|
||
w = int(np.ceil(span[0] / res_out)) + 1
|
||
h = int(np.ceil(span[1] / res_out)) + 1
|
||
return mn, mx, float(res_out), int(w), int(h)
|
||
|
||
def update(
|
||
self,
|
||
stable_points_world: Optional[np.ndarray],
|
||
live_points_world: Optional[np.ndarray],
|
||
now: float,
|
||
) -> Optional[Dict[str, Any]]:
|
||
if not self.cfg.enabled:
|
||
return None
|
||
|
||
static_pts = self._clip_nav_band(stable_points_world)
|
||
live_pts = self._clip_nav_band(live_points_world)
|
||
|
||
bounds = self._compute_bounds(static_pts, live_pts)
|
||
if bounds is None:
|
||
# decay dynamic memory even when there is no input
|
||
self._dynamic_cells = {
|
||
k: v
|
||
for k, v in self._dynamic_cells.items()
|
||
if (float(now) - float(v[1])) <= float(self.cfg.dynamic_decay_sec)
|
||
}
|
||
return None
|
||
|
||
mn, mx = bounds
|
||
mn, mx, res, w, h = self._clamp_grid_size(mn, mx, float(self.cfg.resolution_m))
|
||
|
||
static_occ = np.zeros((h, w), dtype=bool)
|
||
if len(static_pts) > 0:
|
||
gx, gy = self._grid_indices(static_pts[:, :2], mn, res, w, h)
|
||
static_occ[gy, gx] = True
|
||
|
||
# age out dynamic memory first
|
||
max_age = float(self.cfg.dynamic_decay_sec)
|
||
new_dyn: Dict[Tuple[int, int], Tuple[int, float, float, float]] = {}
|
||
dropped_shape = 0
|
||
for key, value in self._dynamic_cells.items():
|
||
try:
|
||
hits, ts, wx, wy = value
|
||
except (ValueError, TypeError):
|
||
# Tuple shape mismatch (legacy 2-tuple from a prior format,
|
||
# or a corrupted entry). Loud-fail rather than silently
|
||
# erasing dynamic obstacle memory.
|
||
dropped_shape += 1
|
||
continue
|
||
if (float(now) - float(ts)) <= max_age:
|
||
new_dyn[key] = (int(hits), float(ts), float(wx), float(wy))
|
||
if dropped_shape:
|
||
_log.warning(
|
||
"_dynamic_cells dropped %d entries with unexpected tuple shape "
|
||
"— check version drift between writers and readers",
|
||
dropped_shape,
|
||
)
|
||
self._dynamic_cells = new_dyn
|
||
|
||
if len(live_pts) > 0:
|
||
# Vectorised hit accumulation per dynamic-key cell. Aggregate
|
||
# via np.unique on stacked (kx, ky) pairs — the previous
|
||
# `(kx<<32) | (ky & 0xFFFFFFFF)` int-packing collided sign-bits
|
||
# at large grids (`ky=-1` aliased to `ky=4294967295`).
|
||
res_key = max(0.02, float(self.cfg.resolution_m))
|
||
kx_all = np.floor(live_pts[:, 0].astype(np.float64) / res_key).astype(np.int64)
|
||
ky_all = np.floor(live_pts[:, 1].astype(np.float64) / res_key).astype(np.int64)
|
||
gx_l, gy_l = self._grid_indices(live_pts[:, :2], mn, res, w, h)
|
||
in_grid = ~static_occ[gy_l, gx_l]
|
||
if np.any(in_grid):
|
||
kx_in = kx_all[in_grid]
|
||
ky_in = ky_all[in_grid]
|
||
wx_in = live_pts[in_grid, 0].astype(np.float64)
|
||
wy_in = live_pts[in_grid, 1].astype(np.float64)
|
||
# `idx` is into the *filtered* arrays — read kx_in / ky_in
|
||
# / wx_in / wy_in at those indices to stay self-consistent
|
||
# without relying on operation ordering.
|
||
pairs = np.column_stack([kx_in, ky_in])
|
||
_, idx, counts = np.unique(
|
||
pairs, axis=0, return_index=True, return_counts=True,
|
||
)
|
||
for u_idx, u_count in zip(idx.tolist(), counts.tolist()):
|
||
cell = (int(kx_in[u_idx]), int(ky_in[u_idx]))
|
||
wx = float(wx_in[u_idx])
|
||
wy = float(wy_in[u_idx])
|
||
old = self._dynamic_cells.get(cell)
|
||
old_hits = int(old[0]) if old is not None else 0
|
||
self._dynamic_cells[cell] = (
|
||
old_hits + int(u_count), float(now), wx, wy,
|
||
)
|
||
|
||
dynamic_occ = np.zeros((h, w), dtype=bool)
|
||
min_hits = int(self.cfg.dynamic_min_hits)
|
||
for _key, (hits, ts, wx, wy) in self._dynamic_cells.items():
|
||
if hits >= min_hits and (float(now) - float(ts)) <= max_age:
|
||
cell = _world_to_grid_strict(wx, wy, mn, res, w, h)
|
||
if cell is not None:
|
||
dynamic_occ[cell[1], cell[0]] = True
|
||
|
||
occ = static_occ | dynamic_occ
|
||
inf_cells = int(np.ceil(float(self.cfg.inflation_radius_m) / res))
|
||
inflated = _inflate_binary(occ, inf_cells)
|
||
|
||
# Cost layering: inflated >= blocked_cost so the local planner +
|
||
# safety supervisor refuse to drive through the inflation buffer.
|
||
# Previously inflation was 180 (advisory) while blocked_cost
|
||
# defaulted to 220 — the planner happily routed through inflated
|
||
# cells, defeating the configured 25 cm inflation radius.
|
||
cost = np.zeros((h, w), dtype=np.uint8)
|
||
blocked = int(self.cfg.blocked_cost)
|
||
cost[inflated] = blocked
|
||
cost[dynamic_occ] = max(blocked, 230)
|
||
cost[static_occ] = 255
|
||
|
||
out = {
|
||
"costmap": cost,
|
||
"static_mask": static_occ,
|
||
"dynamic_mask": dynamic_occ,
|
||
"inflated_mask": inflated,
|
||
"origin_xy": mn.astype(np.float32),
|
||
"resolution_m": float(res),
|
||
"shape_hw": [int(h), int(w)],
|
||
"static_cells": int(np.count_nonzero(static_occ)),
|
||
"dynamic_cells": int(np.count_nonzero(dynamic_occ)),
|
||
"inflated_cells": int(np.count_nonzero(inflated)),
|
||
}
|
||
self._grid = out
|
||
return {
|
||
"shape": [int(h), int(w)],
|
||
"resolution_m": float(res),
|
||
"origin_xy": [float(mn[0]), float(mn[1])],
|
||
"static_cells": int(out["static_cells"]),
|
||
"dynamic_cells": int(out["dynamic_cells"]),
|
||
"inflated_cells": int(out["inflated_cells"]),
|
||
}
|
||
|
||
@property
|
||
def grid(self) -> Optional[Dict[str, Any]]:
|
||
return self._grid
|
||
|
||
|
||
class GlobalAStarPlanner:
|
||
"""Grid A* with corner-cut prevention and a cost-aware admissible
|
||
heuristic.
|
||
|
||
Heuristic admissibility: edge cost is `step + (cost/255)*2.0`, so the
|
||
minimum possible per-step cost is `step` (when cost=0). The Euclidean
|
||
heuristic on grid distance with diagonal step `sqrt(2)` is a lower
|
||
bound on the sum-of-steps any reachable path must traverse, hence
|
||
`h(n) <= true_cost(n→goal)`. The previous implementation kept the
|
||
same heuristic but allowed the cost penalty to push `g_score` above
|
||
the heuristic — still admissible because the heuristic ignored that
|
||
penalty entirely. Documented here so a future tweak doesn't break it.
|
||
"""
|
||
|
||
def __init__(self, blocked_cost: int = 220):
|
||
self.blocked_cost = int(np.clip(int(blocked_cost), 100, 255))
|
||
# Corner-cut threshold separate from `blocked_cost` so a future
|
||
# change to graduated inflation (cells set below blocked_cost)
|
||
# cannot reintroduce diagonal slip through the inflation buffer.
|
||
# Today inflation = blocked_cost so the values coincide.
|
||
self.corner_cut_cost = int(self.blocked_cost)
|
||
# Cost-weight applied per step. Penalty is step-proportional so
|
||
# diagonals through clutter cost more than cardinals through the
|
||
# same clutter — keeps planner preference stable with path length.
|
||
self.cost_weight = 2.0
|
||
|
||
@staticmethod
|
||
def _heur(a: Tuple[int, int], b: Tuple[int, int]) -> float:
|
||
return float(math.hypot(float(a[0] - b[0]), float(a[1] - b[1])))
|
||
|
||
@staticmethod
|
||
def _find_nearest_safe(
|
||
cost: np.ndarray,
|
||
goal: Tuple[int, int],
|
||
max_radius_cells: int,
|
||
blocked_cost: int,
|
||
) -> Optional[Tuple[int, int]]:
|
||
"""Spiral outward from ``goal`` to find the nearest cell whose cost
|
||
is below ``blocked_cost``. Used when the operator clicked a goal
|
||
that falls inside a wall, table, or other obstacle — instead of
|
||
refusing the request, we snap to the nearest cell the robot can
|
||
actually occupy, so the robot approaches the obstacle's boundary
|
||
and stops there rather than pushing into it indefinitely.
|
||
|
||
Returns ``None`` if no safe cell exists within the search radius.
|
||
"""
|
||
h, w = cost.shape
|
||
gx, gy = goal
|
||
for r in range(1, int(max_radius_cells) + 1):
|
||
for dy in range(-r, r + 1):
|
||
for dx in range(-r, r + 1):
|
||
# Only check cells on the perimeter of the current
|
||
# ring; interior cells were already checked in prior
|
||
# iterations.
|
||
if max(abs(dx), abs(dy)) != r:
|
||
continue
|
||
nx, ny = gx + dx, gy + dy
|
||
if 0 <= nx < w and 0 <= ny < h:
|
||
if int(cost[ny, nx]) < blocked_cost:
|
||
return (nx, ny)
|
||
return None
|
||
|
||
@staticmethod
|
||
def _neighbors(x: int, y: int, w: int, h: int) -> Iterable[Tuple[int, int, float]]:
|
||
for dy in (-1, 0, 1):
|
||
for dx in (-1, 0, 1):
|
||
if dx == 0 and dy == 0:
|
||
continue
|
||
nx = x + dx
|
||
ny = y + dy
|
||
if 0 <= nx < w and 0 <= ny < h:
|
||
step = 1.41421356 if (dx != 0 and dy != 0) else 1.0
|
||
yield nx, ny, step
|
||
|
||
def plan(
|
||
self,
|
||
costmap: np.ndarray,
|
||
origin_xy: np.ndarray,
|
||
resolution_m: float,
|
||
start_xy: Tuple[float, float],
|
||
goal_xy: Tuple[float, float],
|
||
max_expansions: int = 120000,
|
||
) -> List[Tuple[float, float]]:
|
||
cost = np.asarray(costmap, dtype=np.uint8)
|
||
if cost.ndim != 2:
|
||
return []
|
||
h, w = cost.shape
|
||
origin = np.asarray(origin_xy, dtype=np.float64)
|
||
res = float(resolution_m)
|
||
blocked = int(self.blocked_cost)
|
||
|
||
def to_grid(x: float, y: float) -> Optional[Tuple[int, int]]:
|
||
return _world_to_grid_strict(x, y, origin, res, w, h)
|
||
|
||
def to_world(gx: int, gy: int) -> Tuple[float, float]:
|
||
return (
|
||
float(origin[0] + (float(gx) + 0.5) * res),
|
||
float(origin[1] + (float(gy) + 0.5) * res),
|
||
)
|
||
|
||
s = to_grid(float(start_xy[0]), float(start_xy[1]))
|
||
g = to_grid(float(goal_xy[0]), float(goal_xy[1]))
|
||
if s is None or g is None:
|
||
return []
|
||
if int(cost[s[1], s[0]]) >= blocked:
|
||
# Robot's own cell is blocked — can't plan from inside an
|
||
# obstacle. Caller should re-localize before retrying.
|
||
return []
|
||
if int(cost[g[1], g[0]]) >= blocked:
|
||
# Operator clicked on a wall / table / chair. Snap to the
|
||
# nearest reachable cell within ~2 m so the robot approaches
|
||
# the obstacle's edge and stops, instead of refusing the
|
||
# request entirely or trying to push through.
|
||
snap_radius_cells = max(int(round(2.0 / max(res, 1e-3))), 4)
|
||
snapped = self._find_nearest_safe(cost, g, snap_radius_cells, blocked)
|
||
if snapped is None:
|
||
_log.warning(
|
||
"A* goal in obstacle and no safe cell within %.1f m; refusing",
|
||
snap_radius_cells * res,
|
||
)
|
||
return []
|
||
_log.info(
|
||
"A* goal snapped from blocked cell %s to safe cell %s "
|
||
"(distance: %.2f m)",
|
||
g, snapped,
|
||
math.hypot((snapped[0] - g[0]) * res, (snapped[1] - g[1]) * res),
|
||
)
|
||
g = snapped
|
||
|
||
open_heap: List[Tuple[float, float, Tuple[int, int]]] = []
|
||
heapq.heappush(open_heap, (self._heur(s, g), 0.0, s))
|
||
came_from: Dict[Tuple[int, int], Tuple[int, int]] = {}
|
||
g_score: Dict[Tuple[int, int], float] = {s: 0.0}
|
||
closed: Dict[Tuple[int, int], bool] = {}
|
||
|
||
expansions = 0
|
||
while open_heap:
|
||
_, cur_g, cur = heapq.heappop(open_heap)
|
||
if closed.get(cur, False):
|
||
continue
|
||
closed[cur] = True
|
||
if cur == g:
|
||
break
|
||
expansions += 1
|
||
if expansions >= int(max_expansions):
|
||
# Route through the module logger so this lands in
|
||
# logs/lidar.log alongside every other SLAM diagnostic.
|
||
# The previous `print(..., file=sys.stderr)` was filtered
|
||
# out of the SDK-pipe so operators never saw it.
|
||
_log.warning(
|
||
"A* expansion limit %d reached; no path found from %s to %s",
|
||
max_expansions, start_xy, goal_xy,
|
||
)
|
||
return []
|
||
|
||
cx, cy = cur
|
||
for nx, ny, step in self._neighbors(cx, cy, w, h):
|
||
cell_cost = int(cost[ny, nx])
|
||
if cell_cost >= blocked:
|
||
continue
|
||
# Corner-cut prevention: a diagonal move from (cx, cy) to
|
||
# (nx, ny) cannot pass through a blocked cardinal cell.
|
||
# Threshold is `corner_cut_cost`, decoupled from `blocked`
|
||
# so a future graduated-inflation refactor cannot
|
||
# reintroduce diagonal slip through the inflation buffer.
|
||
if nx != cx and ny != cy:
|
||
if (
|
||
int(cost[cy, nx]) >= self.corner_cut_cost
|
||
or int(cost[ny, cx]) >= self.corner_cut_cost
|
||
):
|
||
continue
|
||
# Step-proportional cost penalty: a high-cost cell costs
|
||
# ~3× a clear cell to traverse, regardless of cardinal vs
|
||
# diagonal direction. Admissibility holds because penalty
|
||
# ≥ 0 and Euclidean heuristic ignores it.
|
||
ng = float(
|
||
cur_g + step * (1.0 + self.cost_weight * float(cell_cost) / 255.0)
|
||
)
|
||
node = (nx, ny)
|
||
if ng < float(g_score.get(node, 1e18)):
|
||
g_score[node] = ng
|
||
came_from[node] = cur
|
||
f = ng + self._heur(node, g)
|
||
heapq.heappush(open_heap, (f, ng, node))
|
||
|
||
if g not in came_from and g != s:
|
||
return []
|
||
|
||
path_cells: List[Tuple[int, int]] = [g]
|
||
cur = g
|
||
visited_back: set = {g}
|
||
while cur != s:
|
||
nxt = came_from.get(cur)
|
||
if nxt is None or nxt in visited_back:
|
||
# Cycle / missing predecessor implies a corrupted came_from
|
||
# dict (concurrent costmap reset, partial mutation) — surface
|
||
# it loudly so the failure mode is visible in logs rather
|
||
# than silently surfacing as "goal unreachable" upstream.
|
||
_log.warning(
|
||
"A* path reconstruction aborted: %s at cell %s "
|
||
"(start=%s, goal=%s)",
|
||
"cycle in came_from" if nxt in visited_back else "missing predecessor",
|
||
cur, s, g,
|
||
)
|
||
return []
|
||
visited_back.add(nxt)
|
||
cur = nxt
|
||
path_cells.append(cur)
|
||
path_cells.reverse()
|
||
return [to_world(px, py) for (px, py) in path_cells]
|
||
|
||
|
||
@dataclass
|
||
class LocalPlannerConfig:
|
||
lookahead_m: float = 0.8
|
||
max_linear_mps: float = 0.6
|
||
max_angular_rps: float = 1.3
|
||
goal_tolerance_m: float = 0.30
|
||
collision_probe_m: float = 0.6
|
||
# Holonomic evasion — pick from forward / strafe / backward / rotate
|
||
# candidates when the planned forward path is blocked. Capped well
|
||
# below max_linear_mps because G1's RL gait was trained on a forward-
|
||
# heavy velocity envelope; pushing lateral/backward too hard risks an
|
||
# ungainly stumble.
|
||
max_lateral_mps: float = 0.30
|
||
max_backward_mps: float = 0.30
|
||
evasion_enabled: bool = True
|
||
# Whitelist of workflow profile names where holonomic evasion is
|
||
# allowed to fire. An empty list = NO workflow allows evasion (only
|
||
# the autonomous_active override can enable it). To kill evasion
|
||
# entirely, set evasion_enabled=false. Mapping workflows (MAP_NEW /
|
||
# EXTEND_MAP) are intentionally absent from the default so operator
|
||
# teleop isn't fought by auto-avoidance.
|
||
evasion_workflows: List[str] = field(default_factory=lambda: [
|
||
"LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP",
|
||
])
|
||
evasion_lookahead_s: float = 0.5
|
||
# Stuck recovery — fires only after holonomic evasion has produced
|
||
# zero progress for this many seconds. BACKUP then ROTATE then retry,
|
||
# up to N attempts before disarming the drive entirely.
|
||
stuck_trigger_seconds: float = 2.0
|
||
stuck_backup_seconds: float = 1.0
|
||
stuck_rotate_seconds: float = 1.5
|
||
stuck_max_attempts: int = 3
|
||
|
||
@staticmethod
|
||
def from_dict(d: Dict[str, Any] | None) -> "LocalPlannerConfig":
|
||
src = d or {}
|
||
wf_raw = src.get("evasion_workflows")
|
||
if wf_raw is None:
|
||
wf = ["LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP"]
|
||
else:
|
||
try:
|
||
wf = [str(w).upper().strip() for w in wf_raw if str(w).strip()]
|
||
except Exception:
|
||
wf = ["LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP"]
|
||
return LocalPlannerConfig(
|
||
lookahead_m=max(0.1, _safe_float(src.get("lookahead_m", 0.8), 0.8)),
|
||
max_linear_mps=max(0.05, _safe_float(src.get("max_linear_mps", 0.6), 0.6)),
|
||
max_angular_rps=max(0.1, _safe_float(src.get("max_angular_rps", 1.3), 1.3)),
|
||
goal_tolerance_m=max(0.05, _safe_float(src.get("goal_tolerance_m", 0.30), 0.30)),
|
||
collision_probe_m=max(0.1, _safe_float(src.get("collision_probe_m", 0.6), 0.6)),
|
||
max_lateral_mps=max(0.0, _safe_float(src.get("max_lateral_mps", 0.30), 0.30)),
|
||
max_backward_mps=max(0.0, _safe_float(src.get("max_backward_mps", 0.30), 0.30)),
|
||
evasion_enabled=_as_bool(src.get("evasion_enabled", True), True),
|
||
evasion_workflows=wf,
|
||
evasion_lookahead_s=max(0.1, _safe_float(src.get("evasion_lookahead_s", 0.5), 0.5)),
|
||
stuck_trigger_seconds=max(0.5, _safe_float(src.get("stuck_trigger_seconds", 2.0), 2.0)),
|
||
stuck_backup_seconds=max(0.2, _safe_float(src.get("stuck_backup_seconds", 1.0), 1.0)),
|
||
stuck_rotate_seconds=max(0.2, _safe_float(src.get("stuck_rotate_seconds", 1.5), 1.5)),
|
||
stuck_max_attempts=max(0, _safe_int(src.get("stuck_max_attempts", 3), 3)),
|
||
)
|
||
|
||
|
||
class LocalReactivePlanner:
|
||
def __init__(self, cfg: LocalPlannerConfig):
|
||
self.cfg = cfg
|
||
# Stuck-recovery state. ``_stuck_start_time`` is the wall-clock
|
||
# time at which every evasion candidate first started failing;
|
||
# reset to None whenever the robot makes progress. The phase
|
||
# field is the FSM mode ("none" / "backup" / "rotate"). The
|
||
# attempt counter is the only piece that survives a successful
|
||
# FSM cycle — so three full BACKUP→ROTATE cycles with no real
|
||
# progress trip the give-up branch.
|
||
self._stuck_start_time: Optional[float] = None
|
||
self._recovery_phase: str = "none"
|
||
self._recovery_phase_start: float = 0.0
|
||
self._recovery_attempts: int = 0
|
||
self._recovery_rotate_dir: int = 1 # +1 = left, -1 = right
|
||
|
||
@staticmethod
|
||
def _yaw_from_pose(pose: np.ndarray) -> float:
|
||
# Canonical extraction lives in SLAM_Transforms; thin wrapper kept
|
||
# for backward-compat with callers that already use this name.
|
||
from SLAM_Transforms import yaw_rad_from_tf
|
||
return yaw_rad_from_tf(pose)
|
||
|
||
@staticmethod
|
||
def _wrap_pi(a: float) -> float:
|
||
# Constant-time wrap; the previous `while` form was O(|a|) for
|
||
# unwrapped accumulating angles.
|
||
return (float(a) + math.pi) % (2.0 * math.pi) - math.pi
|
||
|
||
def compute_command(
|
||
self,
|
||
pose_world: np.ndarray,
|
||
path_world: List[Tuple[float, float]],
|
||
runtime: LiveCostmapRuntime,
|
||
workflow: str = "",
|
||
autonomous_active: bool = False,
|
||
) -> Dict[str, Any]:
|
||
"""Produce a robot velocity command given pose, path, and costmap.
|
||
|
||
Layered behaviour:
|
||
1. Standard pure pursuit on ``path_world`` → desired (vx, vyaw).
|
||
Decelerates as we approach the goal.
|
||
2. Forward arc-probe: simulate the planned (vx, vyaw) ahead and
|
||
check the costmap. If clear → emit it (today's path).
|
||
3. **Holonomic evasion** (only in workflows listed in
|
||
``cfg.evasion_workflows``): if forward is blocked, sample
|
||
5 candidate motions — forward, strafe-L, strafe-R, backward,
|
||
rotate-only — and pick the one that's clear AND makes the
|
||
most goal progress.
|
||
4. **Stuck recovery** (same workflow gate): if every candidate
|
||
is blocked for ``stuck_trigger_seconds``, fire a deterministic
|
||
BACKUP→ROTATE→retry FSM, up to ``stuck_max_attempts`` times.
|
||
After that, disarm via ``blocked=True`` + ``motion="give_up"``.
|
||
|
||
Outside the evasion workflows (MAP_NEW / EXTEND_MAP) only steps
|
||
1 + 2 run — operator teleop is never overridden.
|
||
"""
|
||
import time as _time
|
||
now = float(_time.time())
|
||
|
||
cmd = {
|
||
"linear_mps": 0.0,
|
||
"lateral_mps": 0.0,
|
||
"angular_rps": 0.0,
|
||
"goal_reached": False,
|
||
"blocked": False,
|
||
"motion": "idle",
|
||
}
|
||
if pose_world is None or np.asarray(pose_world).shape != (4, 4):
|
||
return cmd
|
||
if not path_world:
|
||
return cmd
|
||
|
||
p = np.asarray(pose_world, dtype=np.float64)
|
||
x = float(p[0, 3])
|
||
y = float(p[1, 3])
|
||
yaw = self._yaw_from_pose(p)
|
||
|
||
goal_x, goal_y = float(path_world[-1][0]), float(path_world[-1][1])
|
||
dist_goal = float(math.hypot(goal_x - x, goal_y - y))
|
||
if dist_goal <= float(self.cfg.goal_tolerance_m):
|
||
cmd["goal_reached"] = True
|
||
cmd["motion"] = "arrived"
|
||
self._reset_recovery_state()
|
||
return cmd
|
||
|
||
# ── Evasion / recovery gate ──
|
||
# Holonomic evasion + stuck-recovery are normally restricted to
|
||
# nav workflows. The `autonomous_active` flag is an override —
|
||
# when the autonomous wander system is driving the robot in a
|
||
# mapping workflow (MAP_NEW / EXTEND_MAP), there's no operator
|
||
# teleop to fight, so evasion + recovery should fire. This is
|
||
# the explicit "robot is in charge" gate, separate from the
|
||
# workflow-based gate.
|
||
wf_upper = str(workflow).upper().strip()
|
||
evasion_workflows = set(self.cfg.evasion_workflows or [])
|
||
# An empty `evasion_workflows` list means no workflow opts in to
|
||
# evasion (operator must rely on autonomous_active to override).
|
||
# To disable evasion entirely use evasion_enabled=false.
|
||
evasion_active = (
|
||
bool(self.cfg.evasion_enabled)
|
||
and (
|
||
bool(autonomous_active)
|
||
or (wf_upper in evasion_workflows)
|
||
)
|
||
)
|
||
|
||
# If evasion just gated off (workflow change, wander disarmed,
|
||
# localization lost) while a recovery FSM was in flight, drop
|
||
# the FSM state — otherwise the phase + attempt counter survive
|
||
# into the next evasion-allowed session and bias it toward
|
||
# premature give-up. The state is only meaningful as a continuous
|
||
# sequence within a single evasion-enabled window.
|
||
if not evasion_active and self._recovery_phase != "none":
|
||
self._reset_recovery_state()
|
||
|
||
# If we're already in active recovery and evasion is allowed,
|
||
# the FSM takes priority over normal navigation (it owns the
|
||
# actuator until it completes or finds a clear path).
|
||
if evasion_active and self._recovery_phase != "none":
|
||
rec = self._tick_recovery(now, x, y, yaw, runtime)
|
||
if rec is not None:
|
||
cmd.update(rec)
|
||
return cmd
|
||
# FSM finished its sequence — fall through to normal nav and
|
||
# see if the world has opened up.
|
||
|
||
# ── Pure pursuit → desired (lin, ang) ──
|
||
target = path_world[-1]
|
||
for wx, wy in path_world:
|
||
d = float(math.hypot(float(wx) - x, float(wy) - y))
|
||
if d >= float(self.cfg.lookahead_m):
|
||
target = (float(wx), float(wy))
|
||
break
|
||
|
||
dx = float(target[0] - x)
|
||
dy = float(target[1] - y)
|
||
target_yaw = float(math.atan2(dy, dx))
|
||
yaw_err = self._wrap_pi(target_yaw - yaw)
|
||
|
||
ang = float(np.clip(1.5 * yaw_err, -float(self.cfg.max_angular_rps), float(self.cfg.max_angular_rps)))
|
||
lin_scale = max(0.0, 1.0 - min(1.0, abs(yaw_err) / math.pi))
|
||
lin = float(self.cfg.max_linear_mps) * lin_scale
|
||
|
||
# Smooth deceleration as we approach the goal — same reason as
|
||
# before: avoids overshoot when pure pursuit snaps to zero.
|
||
slow_radius = max(2.0 * float(self.cfg.goal_tolerance_m), 0.7)
|
||
if dist_goal < slow_radius:
|
||
approach_scale = max(0.20, dist_goal / slow_radius)
|
||
lin *= approach_scale
|
||
|
||
# ── Forward probe ──
|
||
forward_blocked = self._is_motion_blocked(
|
||
x, y, yaw, lin, 0.0, ang, runtime,
|
||
horizon_s=float(self.cfg.collision_probe_m) / max(0.1, float(self.cfg.max_linear_mps)),
|
||
)
|
||
|
||
if not forward_blocked:
|
||
cmd["linear_mps"] = float(lin)
|
||
cmd["angular_rps"] = float(ang)
|
||
cmd["motion"] = "forward"
|
||
# Making progress → reset any pending recovery state.
|
||
self._reset_recovery_state()
|
||
return cmd
|
||
|
||
# Forward is blocked. If evasion is gated off (mapping workflows),
|
||
# fall back to today's behaviour: emit zero, blocked=True.
|
||
if not evasion_active:
|
||
cmd["blocked"] = True
|
||
cmd["motion"] = "blocked_no_evasion"
|
||
return cmd
|
||
|
||
# ── Holonomic evasion: sample candidates ──
|
||
best = self._best_evasion_candidate(x, y, yaw, runtime, target)
|
||
if best is not None:
|
||
cmd["linear_mps"] = float(best["vx"])
|
||
cmd["lateral_mps"] = float(best["vy"])
|
||
cmd["angular_rps"] = float(best["vyaw"])
|
||
cmd["motion"] = str(best["name"])
|
||
# Robot is moving (any direction) → not stuck. Reset the full
|
||
# recovery state — including the attempt counter — so a later
|
||
# genuine stuck event gets a fresh quota of attempts. Matches
|
||
# the contract documented on _reset_recovery_state.
|
||
self._reset_recovery_state()
|
||
return cmd
|
||
|
||
# ── No candidate clear → track stuck time + maybe enter recovery ──
|
||
if self._stuck_start_time is None:
|
||
self._stuck_start_time = now
|
||
stuck_duration = now - float(self._stuck_start_time)
|
||
|
||
if stuck_duration >= float(self.cfg.stuck_trigger_seconds):
|
||
if self._recovery_attempts < int(self.cfg.stuck_max_attempts):
|
||
self._start_recovery(now, x, y, yaw, runtime)
|
||
rec = self._tick_recovery(now, x, y, yaw, runtime)
|
||
if rec is not None:
|
||
cmd.update(rec)
|
||
return cmd
|
||
else:
|
||
# Three attempts, still stuck — disarm.
|
||
_log.warning(
|
||
"stuck recovery exhausted (%d attempts); disarming",
|
||
self._recovery_attempts,
|
||
)
|
||
cmd["blocked"] = True
|
||
cmd["motion"] = "give_up"
|
||
return cmd
|
||
|
||
# Still inside the trigger window — emit zero, wait it out.
|
||
cmd["blocked"] = True
|
||
cmd["motion"] = "blocked"
|
||
return cmd
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# Helpers: motion simulation + candidate scoring
|
||
# ─────────────────────────────────────────────────────────────────
|
||
def _is_motion_blocked(
|
||
self,
|
||
x: float, y: float, yaw: float,
|
||
vx_body: float, vy_body: float, vyaw: float,
|
||
runtime: LiveCostmapRuntime,
|
||
horizon_s: float = 0.5,
|
||
n_probe: int = 6,
|
||
) -> bool:
|
||
"""Simulate (vx_body, vy_body, vyaw) ahead through the costmap.
|
||
|
||
Returns True if any probe step lands on a blocked cell. Treats
|
||
vx_body/vy_body in the robot's body frame (so vy is "strafe left
|
||
when positive", vx is "forward when positive"). Yaw is in world
|
||
frame.
|
||
"""
|
||
if runtime is None or getattr(runtime, "grid", None) is None:
|
||
return False
|
||
dt = float(max(0.05, horizon_s)) / float(max(1, n_probe))
|
||
blocked = int(runtime.cfg.blocked_cost)
|
||
lateral_r = float(runtime.cfg.inflation_radius_m) + 0.10
|
||
# The cardinal-point check (footprint sample at ±lateral_r along
|
||
# body axes) is needed whenever the robot's footprint could clip
|
||
# a wall the centerline doesn't see. Two scenarios qualify:
|
||
# 1. Near-stationary pivot (vyaw spinning the body in place)
|
||
# 2. Real lateral motion (strafe) where the side of the robot
|
||
# leads into the inflation buffer ahead of the centerline.
|
||
# Original code only fired in case 1, which let strafe candidates
|
||
# at 0.2 m/s drive a hip into a wall the inflation kept clear of
|
||
# the centerline. Fire when either condition holds.
|
||
max_lin = max(0.1, float(self.cfg.max_linear_mps))
|
||
max_lat = max(0.1, float(self.cfg.max_lateral_mps))
|
||
near_pivot = (abs(vx_body) < 0.05 * max_lin
|
||
and abs(vy_body) < 0.05 * max_lat
|
||
and abs(vyaw) > 1e-3)
|
||
strafing = abs(vy_body) > 0.20 * max_lat
|
||
do_cardinal = near_pivot or strafing
|
||
sx, sy, syaw = float(x), float(y), float(yaw)
|
||
for _ in range(int(n_probe)):
|
||
syaw += vyaw * dt
|
||
# Body-frame velocities → world-frame displacements via 2D
|
||
# rotation: world_x = vx*cos(yaw) - vy*sin(yaw).
|
||
sx += (vx_body * math.cos(syaw) - vy_body * math.sin(syaw)) * dt
|
||
sy += (vx_body * math.sin(syaw) + vy_body * math.cos(syaw)) * dt
|
||
if runtime.cost_at_world(sx, sy) >= blocked:
|
||
return True
|
||
if do_cardinal:
|
||
for ka_off in (0.0, 0.5 * math.pi, math.pi, 1.5 * math.pi):
|
||
ka = syaw + ka_off
|
||
if runtime.cost_at_world(
|
||
sx + lateral_r * math.cos(ka),
|
||
sy + lateral_r * math.sin(ka),
|
||
) >= blocked:
|
||
return True
|
||
return False
|
||
|
||
def _best_evasion_candidate(
|
||
self,
|
||
x: float, y: float, yaw: float,
|
||
runtime: LiveCostmapRuntime,
|
||
target: Tuple[float, float],
|
||
) -> Optional[Dict[str, Any]]:
|
||
"""Sample five candidate body-frame motions, simulate each
|
||
forward by `evasion_lookahead_s`, keep only those that don't hit
|
||
a blocked cell, and pick the one that gets us closest to the
|
||
path's lookahead target.
|
||
|
||
Returns the winning candidate dict, or ``None`` if every option
|
||
is blocked.
|
||
"""
|
||
lookahead = float(self.cfg.evasion_lookahead_s)
|
||
max_lin = float(self.cfg.max_linear_mps)
|
||
max_lat = float(self.cfg.max_lateral_mps)
|
||
max_bwd = float(self.cfg.max_backward_mps)
|
||
max_ang = float(self.cfg.max_angular_rps)
|
||
|
||
# Body-frame candidates. Forward gets a slight score bonus so it
|
||
# wins ties against lateral motion (which looks "spooky" on a
|
||
# humanoid and should be a fallback, not preferred).
|
||
candidates: List[Dict[str, Any]] = [
|
||
{"name": "forward", "vx": +max_lin * 0.7, "vy": 0.0, "vyaw": 0.0, "bias": 0.30},
|
||
{"name": "strafe_left", "vx": 0.0, "vy": +max_lat, "vyaw": 0.0, "bias": 0.0},
|
||
{"name": "strafe_right","vx": 0.0, "vy": -max_lat, "vyaw": 0.0, "bias": 0.0},
|
||
{"name": "backward", "vx": -max_bwd, "vy": 0.0, "vyaw": 0.0, "bias": -0.30},
|
||
{"name": "rotate_left", "vx": 0.0, "vy": 0.0, "vyaw": +max_ang * 0.5, "bias": -0.20},
|
||
{"name": "rotate_right","vx": 0.0, "vy": 0.0, "vyaw": -max_ang * 0.5, "bias": -0.20},
|
||
]
|
||
|
||
best: Optional[Dict[str, Any]] = None
|
||
best_score = -1e9
|
||
tx, ty = float(target[0]), float(target[1])
|
||
|
||
for c in candidates:
|
||
blocked = self._is_motion_blocked(
|
||
x, y, yaw, c["vx"], c["vy"], c["vyaw"], runtime,
|
||
horizon_s=lookahead,
|
||
)
|
||
if blocked:
|
||
continue
|
||
# Simulate end pose to score by goal progress. Use same
|
||
# body→world conversion as the probe.
|
||
n = 4
|
||
dt = lookahead / float(n)
|
||
sx, sy, syaw = x, y, yaw
|
||
for _ in range(n):
|
||
syaw += c["vyaw"] * dt
|
||
sx += (c["vx"] * math.cos(syaw) - c["vy"] * math.sin(syaw)) * dt
|
||
sy += (c["vx"] * math.sin(syaw) + c["vy"] * math.cos(syaw)) * dt
|
||
end_dist = math.hypot(tx - sx, ty - sy)
|
||
start_dist = math.hypot(tx - x, ty - y)
|
||
progress = start_dist - end_dist # positive = closer to target
|
||
score = progress + c["bias"]
|
||
if score > best_score:
|
||
best_score = score
|
||
best = c
|
||
return best
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# Stuck-recovery FSM
|
||
# ─────────────────────────────────────────────────────────────────
|
||
def _reset_recovery_state(self) -> None:
|
||
"""Called whenever the robot is making real progress (forward or
|
||
successful evasion). Clears all stuck/recovery bookkeeping."""
|
||
self._stuck_start_time = None
|
||
self._recovery_phase = "none"
|
||
self._recovery_phase_start = 0.0
|
||
self._recovery_attempts = 0
|
||
|
||
def _start_recovery(
|
||
self, now: float, x: float, y: float, yaw: float,
|
||
runtime: LiveCostmapRuntime,
|
||
) -> None:
|
||
"""Pick the rotate direction by sampling the costmap at ±90°
|
||
relative to current heading, then enter BACKUP phase. Increments
|
||
the attempt counter so the FSM can give up after N tries."""
|
||
# Sample 1.0 m to the left and right; whichever side has a lower
|
||
# cost wins (cheaper neighbourhood = more room to rotate into).
|
||
try:
|
||
r = 1.0
|
||
cost_l = runtime.cost_at_world(
|
||
x + r * math.cos(yaw + math.pi / 2),
|
||
y + r * math.sin(yaw + math.pi / 2),
|
||
)
|
||
cost_r = runtime.cost_at_world(
|
||
x + r * math.cos(yaw - math.pi / 2),
|
||
y + r * math.sin(yaw - math.pi / 2),
|
||
)
|
||
self._recovery_rotate_dir = +1 if cost_l <= cost_r else -1
|
||
except Exception:
|
||
self._recovery_rotate_dir = +1
|
||
self._recovery_phase = "backup"
|
||
self._recovery_phase_start = float(now)
|
||
self._recovery_attempts = int(self._recovery_attempts) + 1
|
||
_log.info(
|
||
"stuck recovery start (attempt %d/%d) — backup then rotate %s",
|
||
self._recovery_attempts,
|
||
int(self.cfg.stuck_max_attempts),
|
||
"left" if self._recovery_rotate_dir > 0 else "right",
|
||
)
|
||
|
||
def _tick_recovery(
|
||
self, now: float, x: float, y: float, yaw: float,
|
||
runtime: LiveCostmapRuntime,
|
||
) -> Optional[Dict[str, Any]]:
|
||
"""Advance the FSM one tick. Returns the velocity dict to emit, or
|
||
None when the FSM has finished its sequence (caller should retry
|
||
normal nav).
|
||
"""
|
||
if self._recovery_phase == "none":
|
||
return None
|
||
elapsed = now - float(self._recovery_phase_start)
|
||
if self._recovery_phase == "backup":
|
||
if elapsed < float(self.cfg.stuck_backup_seconds):
|
||
# Only continue backing up if the path behind is clear.
|
||
if self._is_motion_blocked(
|
||
x, y, yaw,
|
||
-float(self.cfg.max_backward_mps), 0.0, 0.0, runtime,
|
||
horizon_s=0.5,
|
||
):
|
||
# Even backward is blocked — skip straight to rotate.
|
||
self._recovery_phase = "rotate"
|
||
self._recovery_phase_start = float(now)
|
||
return {
|
||
"linear_mps": 0.0,
|
||
"lateral_mps": 0.0,
|
||
"angular_rps": float(self._recovery_rotate_dir)
|
||
* float(self.cfg.max_angular_rps) * 0.5,
|
||
"blocked": False,
|
||
"motion": "recover_rotate",
|
||
}
|
||
return {
|
||
"linear_mps": -float(self.cfg.max_backward_mps) * 0.8,
|
||
"lateral_mps": 0.0,
|
||
"angular_rps": 0.0,
|
||
"blocked": False,
|
||
"motion": "recover_backup",
|
||
}
|
||
# Backup window elapsed → rotate.
|
||
self._recovery_phase = "rotate"
|
||
self._recovery_phase_start = float(now)
|
||
elapsed = 0.0
|
||
if self._recovery_phase == "rotate":
|
||
if elapsed < float(self.cfg.stuck_rotate_seconds):
|
||
return {
|
||
"linear_mps": 0.0,
|
||
"lateral_mps": 0.0,
|
||
"angular_rps": float(self._recovery_rotate_dir)
|
||
* float(self.cfg.max_angular_rps) * 0.5,
|
||
"blocked": False,
|
||
"motion": "recover_rotate",
|
||
}
|
||
# Sequence complete — exit FSM, retry normal navigation. The
|
||
# attempt counter is preserved so subsequent failures trip
|
||
# the give-up threshold.
|
||
self._recovery_phase = "none"
|
||
self._recovery_phase_start = 0.0
|
||
self._stuck_start_time = None
|
||
_log.info("stuck recovery sequence complete; retrying navigation")
|
||
return None
|
||
return None
|