shooty/cerberus.py at master · xoxota99/shooty · 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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
from __future__ import division

import servos
import vision
import sounds
import gun
import random
import logging
from config import cfg

logger = logging.getLogger(__name__)

locked = False
panner = None


def transform(x, y):
    """
    Transform the X/Y coordinates of the target centroid within the camera
    frame into Servo pan/tilt values in degrees.
    """

    return x, y


def target_acquired_callback(target, frame):
    """
    Called from the vision processor:
        - when it discovers a target.
        - continuously, as a target is still in frame.
    """
    global locked
    global panner

    if not locked:
        # New target.
        logger.info("Target Acquired: {0}".format(target))
        panner.pause()
        if target.friendly:
            s = random.choice({
                sounds.HELLO_FRIEND,
                sounds.HELLO_FRIEND_2
            })
            sounds.play(s, False)
        else:
            sounds.play(sounds.TARGET_ACQUIRED, False)

        locked = True

    # aim for the center of mass
    sx, sy = transform(int(target.x + target.w / 2),
                       int(target.y + target.h / 2))

    servos.move_to(sx, sy)
    if not target.friendly:
        gun.shoot()


def target_lost_callback(target, frame):
    global locked
    global panner
    """
    Called from the vision processor:
        - when a target is lost.
        - continuously, as there is no target in frame.
    """
    if locked:
        # we had a target, now we don't.
        logger.info("Target Lost: {0}".format(target))
        panner.pause(False)
        if not target.friendly:
            sounds.play(sounds.TARGET_LOST, False)
        locked = False


"""Main sentry program"""
if (__name__ == "__main__"):
    logger.info("initializing servos")
    servos.init()

    logger.info("moveto(0,0)")
    servos.move_to(0, 0)

    # "panner" is a thread that just moves the pan servo back and forth, in a
    # "scanning for intruders" sort of behavior. It's purely cosmetic, but
    # helps us remember that the bloody thing is on and dangerous.
    panner = servos.ScanWorker()
    panner.daemon = True
    logger.info("starting motor pan/tilt thread.")
    panner.start()

    # "scanner" is the thread that's running the vision main loop and video
    # processing.
    scanner = vision.VisionWorker(
        target_acquired_callback=target_acquired_callback, target_lost_callback=target_lost_callback)
    scanner.daemon = True
    logger.info("starting vision thread.")
    scanner.start()

    while not scanner.stopped:
        val = input("Input Command (enter to fire, 'Q' to quit.): ")

        if(val.lower() == "q"):
            break
        elif(val == ""):
            scanner.pause()

            gun.shoot()

            scanner.pause(False)
        else:
            logger.warn("Unrecognized command: '{0}'".format(val))

    scanner.stop()
    panner.stop()
    servos.shutdown()