97 lines
2.9 KiB
Python
97 lines
2.9 KiB
Python
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
from typing import Any, Dict, List, Optional, Tuple
|
|
|
|
import numpy as np
|
|
|
|
|
|
def _safe_float(v: Any, default: float) -> float:
|
|
try:
|
|
return float(v)
|
|
except Exception:
|
|
return float(default)
|
|
|
|
|
|
@dataclass
|
|
class MissionConfig:
|
|
waypoint_tolerance_m: float = 0.55 # G1 Edu stride ~0.3 m; 0.55 m gives reliable completion
|
|
|
|
@staticmethod
|
|
def from_dict(d: Dict[str, Any] | None) -> "MissionConfig":
|
|
src = d or {}
|
|
return MissionConfig(
|
|
waypoint_tolerance_m=max(0.05, _safe_float(src.get("waypoint_tolerance_m", 0.55), 0.55)),
|
|
)
|
|
|
|
|
|
class WaypointMissionManager:
|
|
def __init__(self, cfg: MissionConfig):
|
|
self.cfg = cfg
|
|
self.reset()
|
|
|
|
def reset(self) -> None:
|
|
self._waypoints: List[Tuple[float, float]] = []
|
|
self._idx = 0
|
|
self._active = False
|
|
self._paused = False
|
|
self._completed = False
|
|
|
|
def set_waypoints(self, waypoints: List[Tuple[float, float]]) -> None:
|
|
self._waypoints = [(float(x), float(y)) for x, y in waypoints]
|
|
self._idx = 0
|
|
self._completed = len(self._waypoints) == 0
|
|
|
|
def start(self, waypoints: Optional[List[Tuple[float, float]]] = None) -> None:
|
|
if waypoints is not None:
|
|
self.set_waypoints(waypoints)
|
|
self._active = len(self._waypoints) > 0
|
|
self._paused = False
|
|
self._completed = not self._active
|
|
|
|
def pause(self) -> None:
|
|
if self._active:
|
|
self._paused = True
|
|
|
|
def resume(self) -> None:
|
|
if self._active:
|
|
self._paused = False
|
|
|
|
def stop(self) -> None:
|
|
self._active = False
|
|
self._paused = False
|
|
|
|
def current_goal(self) -> Optional[Tuple[float, float]]:
|
|
if not self._active or self._paused or self._completed:
|
|
return None
|
|
if self._idx < 0 or self._idx >= len(self._waypoints):
|
|
return None
|
|
return self._waypoints[self._idx]
|
|
|
|
def update_pose(self, x: float, y: float) -> Dict[str, Any]:
|
|
goal = self.current_goal()
|
|
if goal is None:
|
|
return self.snapshot()
|
|
|
|
dx = float(goal[0]) - float(x)
|
|
dy = float(goal[1]) - float(y)
|
|
dist = float(np.hypot(dx, dy))
|
|
if dist <= float(self.cfg.waypoint_tolerance_m):
|
|
self._idx += 1
|
|
if self._idx >= len(self._waypoints):
|
|
self._completed = True
|
|
self._active = False
|
|
self._paused = False
|
|
return self.snapshot()
|
|
|
|
def snapshot(self) -> Dict[str, Any]:
|
|
goal = self.current_goal()
|
|
return {
|
|
"active": bool(self._active),
|
|
"paused": bool(self._paused),
|
|
"completed": bool(self._completed),
|
|
"index": int(self._idx),
|
|
"total": int(len(self._waypoints)),
|
|
"goal": [float(goal[0]), float(goal[1])] if goal is not None else None,
|
|
}
|