67ClockBot/controller.py at main · cogniera/67ClockBot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
# controller.py
# Handles robot behavior: IDLE, RETREAT, SLIDE, BOUNCE.

import time, numpy as np

class RobotController:
    def __init__(self):
        self.mode = "IDLE"
        self.ema_depth = None
        self.last_offset = 0.0
        self.phase_t0 = 0.0

        self.near_thr = 0.55
        self.ema_alpha = 0.35

        self.v_back = -0.25
        self.omega_gain = 1.15

        self.slide_speed = -0.12
        self.bounce_speed = 0.12
        self.safe_omega = 0.9

        self.slide_time = 0.60
        self.bounce_time = 0.50
        self.wall_stop_cm = 20

        self.v_cmd = 0
        self.w_cmd = 0

    # desc: Updates the internal FSM based on vision + ultrasonic.
    # pre: person_box, depth_map may be None; wall_cm may be None.
    # post: Updates v_cmd, w_cmd and returns current mode.
    def update(self, box, depth, wall_cm):
        person_close = False

        if box is not None:
            h, w = depth.shape
            x1, y1, x2, y2 = map(int, (box["x1"], box["y1"], box["x2"], box["y2"]))
            x1, x2 = max(0, x1), min(w - 1, x2)
            y1, y2 = max(0, y1), min(h - 1, y2)

            roi = depth[y1:y2, x1:x2]
            if roi.size > 0:
                med = float(np.median(roi))
                self.ema_depth = med if self.ema_depth is None else (
                    self.ema_alpha * med + (1 - self.ema_alpha) * self.ema_depth
                )

                cx = 0.5 * (x1 + x2)
                self.last_offset = (cx - w / 2) / (w / 2)

                if self.ema_depth >= self.near_thr:
                    person_close = True

        now = time.monotonic()
        m = self.mode

        if m in ("IDLE", "RETREAT"):
            if person_close:
                self.mode = "RETREAT"
                self.v_cmd = self.v_back
                self.w_cmd = -self.last_offset * self.omega_gain

                if wall_cm is not None and wall_cm <= self.wall_stop_cm:
                    self.mode = "SLIDE"
                    self.phase_t0 = now
            else:
                self.mode = "IDLE"
                self.v_cmd = 0
                self.w_cmd = 0

        if self.mode == "SLIDE":
            sign = -1 if self.last_offset > 0 else 1
            self.v_cmd = self.slide_speed
            self.w_cmd = sign * self.safe_omega

            if now - self.phase_t0 >= self.slide_time:
                self.mode = "BOUNCE"
                self.phase_t0 = now

        elif self.mode == "BOUNCE":
            sign = 1 if self.last_offset > 0 else -1
            self.v_cmd = self.bounce_speed
            self.w_cmd = sign * self.safe_omega

            if now - self.phase_t0 >= self.bounce_time:
                self.mode = "RETREAT" if person_close else "IDLE"

        return self.mode

    # desc: Returns the velocity command to send.
    def velocity(self):
        return self.v_cmd, self.w_cmd