67 lines
1.8 KiB
Python
67 lines
1.8 KiB
Python
"""In-process arbitration between Nav2 (web_nav3) and the manual LocoController.
|
|
|
|
Both stacks can drive the G1's legs via different command paths:
|
|
- Nav2 (web_nav3) publishes cmd_vel from a navigation goal/mission.
|
|
- LocoController issues LocoClient.Move()/step() from the Controller tab and
|
|
Gemini movement dispatch.
|
|
|
|
The documented hazard is "two stacks must never both drive the legs at once".
|
|
This module is a tiny thread-safe gate that lets ONE commander own the legs at a
|
|
time. controller.py sets loco_active for arm/move/step and refuses when nav is
|
|
active; navigation.py sets nav_active for goto/missions/run and refuses when loco
|
|
is active. The E-STOP / cancel paths clear the relevant flag.
|
|
|
|
Pure in-process state (no DDS, no HTTP) — both routers share this single module
|
|
instance, so the flags are coherent across the dashboard process.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import threading
|
|
|
|
_lock = threading.Lock()
|
|
_loco_active = False
|
|
_nav_active = False
|
|
|
|
|
|
def loco_active() -> bool:
|
|
with _lock:
|
|
return _loco_active
|
|
|
|
|
|
def nav_active() -> bool:
|
|
with _lock:
|
|
return _nav_active
|
|
|
|
|
|
def acquire_loco() -> bool:
|
|
"""Claim the legs for manual loco. Returns False if Nav2 holds them."""
|
|
global _loco_active
|
|
with _lock:
|
|
if _nav_active:
|
|
return False
|
|
_loco_active = True
|
|
return True
|
|
|
|
|
|
def release_loco() -> None:
|
|
global _loco_active
|
|
with _lock:
|
|
_loco_active = False
|
|
|
|
|
|
def acquire_nav() -> bool:
|
|
"""Claim the legs for Nav2. Returns False if manual loco holds them."""
|
|
global _nav_active
|
|
with _lock:
|
|
if _loco_active:
|
|
return False
|
|
_nav_active = True
|
|
return True
|
|
|
|
|
|
def release_nav() -> None:
|
|
global _nav_active
|
|
with _lock:
|
|
_nav_active = False
|