wlpro-robot-ctl/robot-daemon.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
常驻长连接驱动：保持单条 WebSocket 不断开，持续推零帧，按需注入动作。

设计要点（针对 Arduino WebSocketsServer + 短连接丢尾帧的问题）：
  * 全程只开一条连接，运行期间绝不关闭 → 避免"断开瞬间丢归零帧 → 速度残留"
  * 空闲时每 120ms 推一帧全零（dir=stop, joy=0,0），与网页空闲行为一致
  * 动作靠写状态文件注入，带硬性到期时间，到期自动归零
  * stable 恒为 1（主动保持平衡），本进程拒绝任何关闭请求
  * height 恒为 38，roll 恒为 0

状态文件（默认 ./daemon-state.json）：
  {"joy_x":0, "joy_y":0, "dir":"stop", "until": <epoch秒>}
  控制脚本只写这个文件；本进程读取并执行。

用法：
  python3 robot-daemon.py [--host H] [--port P] [--state FILE] [--interval 0.12]
  # 后台运行建议 nohup + setsid
"""

import argparse
import json
import os
import signal
import sys
import time

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from importlib.machinery import SourceFileLoader

_ctl = SourceFileLoader("rctl", os.path.join(os.path.dirname(os.path.abspath(__file__)), "robot-ctl.py")).load_module()

HEIGHT = 38
ROLL = 0
STABLE = 1

ZERO = {"joy_x": 0, "joy_y": 0, "dir": "stop"}


def log(*a):
    print(time.strftime("[%H:%M:%S]"), *a, flush=True)


def frame_of(joy_x, joy_y, direction):
    return {
        "dir": direction,
        "mode": "basic",
        "roll": ROLL,
        "height": HEIGHT,
        "linear": 0,
        "angular": 0,
        "stable": STABLE,
        "joy_x": joy_x,
        "joy_y": joy_y,
    }


def read_state(path):
    """读状态文件；不存在或损坏则返回零状态"""
    try:
        with open(path, "r", encoding="utf-8") as f:
            d = json.load(f)
        jx = int(d.get("joy_x", 0) or 0)
        jy = int(d.get("joy_y", 0) or 0)
        dr = str(d.get("dir", "stop") or "stop")
        until = float(d.get("until", 0) or 0)
        jx = max(-100, min(100, jx))
        jy = max(-100, min(100, jy))
        if dr not in ("stop", "jump"):
            dr = "stop"
        return {"joy_x": jx, "joy_y": jy, "dir": dr, "until": until}
    except Exception:
        return {"joy_x": 0, "joy_y": 0, "dir": "stop", "until": 0}


class Daemon:
    def __init__(self, host, port, state_path, interval):
        self.host, self.port = host, port
        self.state_path = state_path
        self.interval = interval
        self.ws = None
        self.running = True
        self.last_key = None
        self.sent = 0
        self.connects = 0

    def connect(self):
        while self.running:
            try:
                self.ws = _ctl.WS(self.host, self.port, timeout=8, verbose=False)
                self.ws.connect()
                self.connects += 1
                log(f"已连接 {self.host}:{self.port}（第 {self.connects} 次）")
                return True
            except Exception as e:
                log(f"连接失败: {e}，2 秒后重试")
                time.sleep(2)
        return False

    def run(self):
        if not self.connect():
            return 1
        log(f"常驻开始 | 间隔 {int(self.interval*1000)}ms | stable={STABLE} height={HEIGHT}")
        last_report = time.time()
        try:
            while self.running:
                st = read_state(self.state_path)
                now = time.time()

                # 到期 → 归零
                if st["until"] and now >= st["until"]:
                    st = {"joy_x": 0, "joy_y": 0, "dir": "stop", "until": 0}

                f = frame_of(st["joy_x"], st["joy_y"], st["dir"])
                payload = json.dumps(f, separators=(",", ":"))

                try:
                    self.ws.send_text(payload)
                    self.sent += 1
                except Exception as e:
                    log(f"发送失败: {e} → 重连")
                    try:
                        self.ws.close()
                    except Exception:
                        pass
                    self.ws = None
                    if not self.connect():
                        break
                    continue

                # 状态变化时打日志
                key = (st["joy_x"], st["joy_y"], st["dir"])
                if key != self.last_key:
                    log(f"状态 → joy_x={st['joy_x']} joy_y={st['joy_y']} dir={st['dir']}"
                        + (f" 剩余 {st['until']-now:.1f}s" if st["until"] else ""))
                    self.last_key = key

                if time.time() - last_report >= 30:
                    log(f"心跳 | 已发 {self.sent} 帧 | 状态 {key}")
                    last_report = time.time()

                time.sleep(self.interval)
        except KeyboardInterrupt:
            pass
        finally:
            self.shutdown()
        return 0

    def shutdown(self):
        """退出前归零：多推几帧再关，尽量不让设备残留速度"""
        log("开始收尾：推零帧")
        try:
            zero = json.dumps(frame_of(0, 0, "stop"), separators=(",", ":"))
            for _ in range(12):
                try:
                    self.ws.send_text(zero)
                except Exception:
                    break
                time.sleep(0.12)
            time.sleep(0.5)
        except Exception as e:
            log(f"收尾异常: {e}")
        try:
            if self.ws:
                self.ws.close()
        except Exception:
            pass
        log("已断开")


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--host", default="203.0.113.10")
    ap.add_argument("--port", type=int, default=4782)
    ap.add_argument("--state", default=os.path.join(os.path.dirname(os.path.abspath(__file__)), "daemon-state.json"))
    ap.add_argument("--interval", type=float, default=0.12)
    opts = ap.parse_args()

    d = Daemon(opts.host, opts.port, opts.state, opts.interval)

    def on_sig(signum, _frame):
        d.running = False

    signal.signal(signal.SIGTERM, on_sig)
    signal.signal(signal.SIGINT, on_sig)
    sys.exit(d.run())


if __name__ == "__main__":
    main()