Thermal_Guidance_sim/behavior.py at main · SachinKharote/Thermal_Guidance_sim · 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
# behavior.py
from modes import Mode


class BehaviorController:
    def __init__(
        self,
        lock_confidence=0.15,
        intercept_confidence=0.25
    ):
        self.lock_confidence = lock_confidence
        self.intercept_confidence = intercept_confidence
        self.mode = Mode.SEARCH

    def update(self, confidence, has_measurement, near_wall):
        """
        Decide behavior mode based on:
        - track confidence
        - sensor availability
        - boundary safety
        """

        if near_wall:
            self.mode = Mode.SEARCH
            return self.mode

        if not has_measurement:
            self.mode = Mode.SEARCH
            return self.mode

        if confidence > self.intercept_confidence:
            self.mode = Mode.INTERCEPT
        elif confidence > self.lock_confidence:
            self.mode = Mode.TRACK
        else:
            self.mode = Mode.SEARCH

        return self.mode