50 lines
1.3 KiB
Python
50 lines
1.3 KiB
Python
"""
|
|
zmq_api.py — ZMQ velocity + command interface to Holosoma
|
|
"""
|
|
import json
|
|
import time
|
|
import zmq
|
|
from Core.config_loader import load_config
|
|
from Core.logger import log
|
|
|
|
_cfg = load_config("ZMQ")
|
|
|
|
ZMQ_HOST = _cfg["zmq_host"]
|
|
ZMQ_PORT = _cfg["zmq_port"]
|
|
STOP_ITERATIONS = _cfg["stop_iterations"]
|
|
STOP_DELAY = _cfg["stop_delay"]
|
|
STEP_PAUSE = _cfg["step_pause"]
|
|
|
|
ctx = zmq.Context()
|
|
sock = ctx.socket(zmq.PUB)
|
|
sock.bind(f"tcp://{ZMQ_HOST}:{ZMQ_PORT}")
|
|
time.sleep(0.5)
|
|
log(f"ZMQ PUB bound on tcp://{ZMQ_HOST}:{ZMQ_PORT}", "info", "zmq")
|
|
|
|
|
|
def get_socket():
|
|
"""Return the shared ZMQ PUB socket (for odometry to reuse)."""
|
|
return sock
|
|
|
|
|
|
def send_vel(vx: float = 0.0, vy: float = 0.0, vyaw: float = 0.0):
|
|
"""Send velocity to Holosoma. vx m/s | vy m/s | vyaw rad/s"""
|
|
sock.send_string(json.dumps({"vel": {"vx": vx, "vy": vy, "vyaw": vyaw}}))
|
|
|
|
|
|
def gradual_stop():
|
|
"""Smooth deceleration to zero over ~1 second."""
|
|
for _ in range(STOP_ITERATIONS):
|
|
send_vel(0.0, 0.0, 0.0)
|
|
time.sleep(STOP_DELAY)
|
|
|
|
|
|
def send_cmd(cmd: str):
|
|
"""Send Holosoma state command: start | walk | stand | stop"""
|
|
sock.send_string(json.dumps({"cmd": cmd}))
|
|
|
|
|
|
# Load MOVE_MAP from navigation config
|
|
_nav = load_config("Navigation")
|
|
MOVE_MAP = {k: tuple(v) for k, v in _nav["move_map"].items()}
|