feat(unitree_g1): make onboard state PUB toggleable + lower default rate

Lower observation.state publish rate to 30 Hz and allow disabling it with
--state-fps <=0, so the onboard runner can reproduce the plain-teleop setup
(no state thread) for A/B isolation of camera-stability issues.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Martino Russi
2026-07-19 12:39:28 +02:00
parent 48d8db1f3e
commit 437248d823
@@ -74,7 +74,12 @@ def main() -> None:
default=STATE_PORT, default=STATE_PORT,
help="ZMQ PUB port for observation.state feedback (29 joint .q) to the inference client", help="ZMQ PUB port for observation.state feedback (29 joint .q) to the inference client",
) )
p.add_argument("--state-fps", type=float, default=60.0, help="observation.state publish rate (Hz)") p.add_argument(
"--state-fps",
type=float,
default=30.0,
help="observation.state publish rate (Hz); set <=0 to disable the state PUB entirely",
)
p.add_argument( p.add_argument(
"--gravity-compensation", "--gravity-compensation",
action="store_true", action="store_true",
@@ -152,22 +157,26 @@ def main() -> None:
sock.bind(f"tcp://0.0.0.0:{args.action_port}") sock.bind(f"tcp://0.0.0.0:{args.action_port}")
logger.info("Onboard controller live. Waiting for laptop actions on :%d ...", args.action_port) logger.info("Onboard controller live. Waiting for laptop actions on :%d ...", args.action_port)
stop = threading.Event()
signal.signal(signal.SIGINT, lambda *_: stop.set())
signal.signal(signal.SIGTERM, lambda *_: stop.set())
# Proprioception feedback: publish observation.state (29 joint .q) so the # Proprioception feedback: publish observation.state (29 joint .q) so the
# laptop-side inference client can feed it to the policy. DDS stays local to # laptop-side inference client can feed it to the policy. DDS stays local to
# the robot; only this compact JSON state crosses the network (like the # the robot; only this compact JSON state crosses the network (like the
# camera/action ZMQ channels). # camera/action ZMQ channels). get_observation() here reads only DDS lowstate
# (the robot is built with cameras={}), so it never touches the USB cameras.
# Disable with --state-fps <=0 (e.g. to reproduce the plain-teleop setup).
state_sock = None
if args.state_fps > 0:
state_sock = ctx.socket(zmq.PUB) state_sock = ctx.socket(zmq.PUB)
state_sock.setsockopt(zmq.SNDHWM, 2) state_sock.setsockopt(zmq.SNDHWM, 2)
state_sock.setsockopt(zmq.LINGER, 0) state_sock.setsockopt(zmq.LINGER, 0)
state_sock.bind(f"tcp://0.0.0.0:{args.state_port}") state_sock.bind(f"tcp://0.0.0.0:{args.state_port}")
logger.info("Publishing observation.state on :%d at %.0f Hz", args.state_port, args.state_fps) logger.info("Publishing observation.state on :%d at %.0f Hz", args.state_port, args.state_fps)
stop = threading.Event()
signal.signal(signal.SIGINT, lambda *_: stop.set())
signal.signal(signal.SIGTERM, lambda *_: stop.set())
def publish_state() -> None: def publish_state() -> None:
period = 1.0 / args.state_fps if args.state_fps > 0 else 0.0 period = 1.0 / args.state_fps
joint_names = [j.name for j in G1_29_JointIndex] joint_names = [j.name for j in G1_29_JointIndex]
while not stop.is_set(): while not stop.is_set():
t0 = time.time() t0 = time.time()
@@ -178,10 +187,11 @@ def main() -> None:
state_sock.send_json(state, zmq.NOBLOCK) state_sock.send_json(state, zmq.NOBLOCK)
except zmq.Again: except zmq.Again:
pass pass
if period:
time.sleep(max(0.0, period - (time.time() - t0))) time.sleep(max(0.0, period - (time.time() - t0)))
threading.Thread(target=publish_state, daemon=True).start() threading.Thread(target=publish_state, daemon=True).start()
else:
logger.info("observation.state PUB disabled (--state-fps<=0); inference client will get no proprio")
n = 0 n = 0
try: try:
@@ -234,6 +244,7 @@ def main() -> None:
finally: finally:
logger.info("Shutting down onboard controller...") logger.info("Shutting down onboard controller...")
stop.set() stop.set()
if state_sock is not None:
try: try:
state_sock.close(linger=0) state_sock.close(linger=0)
except Exception as e: # noqa: BLE001 except Exception as e: # noqa: BLE001