"""Keep CMU height hazards, except an observed supported grade. Height above a cell's low quantile is not step height. A smooth 15-degree ramp can exceed 10 cm across that cell. A 6 cm voxel mesh also quantizes a continuous grade. Admit that surface only with broad support, a bounded plane residual and no observed short-range height jump exceeding the qualified 10 cm step. Vertical surfaces, excessive roughness and sparse/unknown support retain the CMU cost. """ import ctypes import hashlib import math from collections import OrderedDict from pathlib import Path import numpy as np _NATIVE_PATH = Path("/opt/missioncore/libterrain_connectivity.so") _NATIVE = ctypes.CDLL(str(_NATIVE_PATH)) if _NATIVE_PATH.is_file() else None if _NATIVE is not None: for suffix, dtype in (("f32", ctypes.c_float), ("f64", ctypes.c_double)): function = getattr(_NATIVE, "terrain_connected_" + suffix) function.argtypes = [ctypes.POINTER(dtype), ctypes.c_int] function.restype = ctypes.c_int def connected_grade(nearby): if _NATIVE is not None and nearby.dtype in (np.dtype("float32"), np.dtype("float64")): points = np.ascontiguousarray(nearby[:, :3]) dtype, suffix = ( (ctypes.c_float, "f32") if points.dtype.itemsize == 4 else (ctypes.c_double, "f64") ) return bool( getattr(_NATIVE, "terrain_connected_" + suffix)( points.ctypes.data_as(ctypes.POINTER(dtype)), len(points) ) ) # Reference implementation retained for portable CPU tests and comparison. dx = nearby[:, None, 0] - nearby[None, :, 0] dy = nearby[:, None, 1] - nearby[None, :, 1] separation = np.sqrt(dx * dx + dy * dy) jump = np.abs(nearby[:, None, 2] - nearby[None, :, 2]) if np.any((separation <= 0.12) & (jump > 0.1001)): return False connected = separation <= 0.12 reached = connected[np.argmin(np.linalg.norm(nearby[:, :2], axis=1))].copy() while True: expanded = np.any(connected[reached], axis=0) if np.array_equal(expanded, reached): return bool(reached.all()) reached = expanded def underbody_support_costs(terrain, pose, contact_height_m=0.37): """Reconcile low returns already inside the current chassis footprint. CMU's neighbourhood ground reference can label the supported terrain under the chassis as a body collision. Use measured pose and the declared contact height only inside the body (with a 5 cm inset), never for terrain ahead. Retain drops, high returns and excessive tilt. The 8 cm band has 2 cm reserve below the physically qualified 10 cm step; raw terrain memory is unchanged. """ result = terrain.copy() x, y, z, w = pose[3:] up = np.array([2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)]) if up[2] < math.cos(math.radians(25)): return result, 0 yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)) axes = np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]]) local = (terrain[:, :2] - pose[:2]) @ axes height = (terrain[:, :3] - pose[:3]) @ up + contact_height_m supported = ( (np.abs(local) < 0.45).all(axis=1) & (np.abs(height) <= 0.08) & (terrain[:, 3] > 0.1) ) result[supported, 3] = np.abs(height[supported]) return result, int(supported.sum()) def _neighborhoods(terrain, radius=0.4): """Exact radius neighborhoods without scanning the whole accumulated map. Returns in the nine adjacent cells include every possible neighbor. Keep source order so fitting and thresholds remain identical to the full scan. """ cells = np.floor(terrain[:, :2] / radius).astype(np.int64) buckets = {} for index, (x, y) in enumerate(cells): buckets.setdefault((x, y), []).append(index) cached = {} def around(index): key = tuple(cells[index]) if key not in cached: x, y = key cached[key] = np.array( sorted( i for dx in (-1, 0, 1) for dy in (-1, 0, 1) for i in buckets.get((x + dx, y + dy), ()) ), dtype=np.int64, ) delta = terrain[cached[key], :3] - terrain[index, :3] return delta[np.linalg.norm(delta[:, :2], axis=1) <= radius] return around def supported_slope_costs(terrain, cache=None): result = terrain.copy() if len(terrain) < 8: return result, 0 around = _neighborhoods(terrain) corrected = 0 for index in np.flatnonzero(terrain[:, 3] > 0.1): nearby = around(index) if len(nearby) < 8: continue # Any admitted plane spans at most a 0.8 m diameter at 25 degrees, # plus the two 7.5 cm residuals. Reject tall foliage/walls before fitting. if np.ptp(nearby[:, 2]) > 0.8 * math.tan(math.radians(25 + 1e-4)) + 0.15: continue key = None if cache is not None: key = hashlib.blake2b(nearby.tobytes(), digest_size=24).digest() if key in cache: if cache[key]: result[index, 3] = 0.0 corrected += 1 cache.move_to_end(key) continue cache[key] = False if len(cache) > 16384: cache.popitem(last=False) # No collinear strip, hidden region, multiple height layers or vertical # surface is admitted as a plane. All observed points must agree. covariance = np.cov(nearby[:, :2], rowvar=False) if np.linalg.eigvalsh(covariance)[0] < 0.0036: continue matrix = np.column_stack((nearby[:, :2], np.ones(len(nearby)))) plane = np.linalg.lstsq(matrix, nearby[:, 2], rcond=None)[0] slope = math.degrees(math.atan(np.linalg.norm(plane[:2]))) if not 2 <= slope <= 25 + 1e-4 or abs(plane[2]) > 0.05: continue if np.max(np.abs(matrix @ plane - nearby[:, 2])) > 0.075: continue # A permissive fit alone could erase a real ledge. Test close measured # returns explicitly: even a narrow step/drop must retain its hazard. if not connected_grade(nearby): continue # Do not fit a road across a gap with no returns. result[index, 3] = 0.0 corrected += 1 if cache is not None: cache[key] = True return result, corrected class TerrainCostNormalizer: """Reuse fits only for byte-identical observed neighborhoods, bounded in RAM. New or changed returns always trigger a new fit. This stores no occupancy belief and clears with the owning ROS node on every episode/reset. """ def __init__(self): self.cache = OrderedDict() def __call__(self, terrain): return supported_slope_costs(terrain, self.cache)