嵌入式视觉模型进阶 17：目标跟随：从轨迹到云台控制接口

"""实现目标锁定、死区、限速和失效停止，分离视觉输出与电机驱动。"""

from controller import Controller


def row(x, ident=1, matched=True):
    return {"id": ident, "box": [x, 0, x + 20, 20], "matched": matched}


def main():
    controller = Controller()
    commands = []
    for x in [70, 65, 60, 45]:
        out = controller.step([row(x)], 100, 0.05, 0.02)
        commands.append(out["speed"])
        print(out)
    assert all(abs(v) <= 0.6 for v in commands)
    assert all(abs(a - b) <= 0.100001 for a, b in zip([0] + commands, commands))
    stopped = controller.step([row(70)], 100, 0.05, 0.3)
    assert stopped["speed"] == 0
    lost = controller.step([row(70, 2)], 100, 0.05, 0.02)
    assert lost["speed"] == 0 and lost["target_id"] == 1
    controller.release()
    assert controller.step([row(70, 2)], 100, 0.05, 0.02)["target_id"] == 2
    print("stale stop, identity lock and explicit reacquisition passed")


if __name__ == "__main__":
    main()

"""输出归一化角速度意图，不直接驱动电机。"""

import math


class Controller:
    def __init__(
        self, kp=1.2, deadzone=0.05, max_speed=0.6, max_accel=2.0, timeout=0.2
    ):
        self.kp = kp
        self.deadzone = deadzone
        self.max_speed = max_speed
        self.max_accel = max_accel
        self.timeout = timeout
        self.previous = 0.0
        self.target = None

    def step(self, rows, width, dt, age):
        if (
            width <= 0
            or dt <= 0
            or not all(math.isfinite(v) for v in [width, dt, age])
            or age < 0
        ):
            raise ValueError("invalid timing or dimensions")
        visible = [r for r in rows if r["matched"]]
        chosen = next((r for r in visible if r["id"] == self.target), None)
        if chosen is None and self.target is None and visible:
            chosen = min(
                visible, key=lambda r: abs((r["box"][0] + r["box"][2]) / 2 - width / 2)
            )
            self.target = chosen["id"]
        if age > self.timeout or chosen is None:
            self.previous = 0.0
            return {"target_id": self.target, "speed": 0.0, "reason": "stale_or_lost"}
        center = (chosen["box"][0] + chosen["box"][2]) / 2
        error = (center - width / 2) / (width / 2)
        desired = (
            0.0
            if abs(error) <= self.deadzone
            else max(-self.max_speed, min(self.max_speed, self.kp * error))
        )
        delta = self.max_accel * dt
        speed = max(self.previous - delta, min(self.previous + delta, desired))
        self.previous = speed
        return {"target_id": self.target, "speed": speed, "reason": "tracking"}

    def release(self):
        self.target = None
        self.previous = 0.0