backend-legacy/coderbot.py at 4.0-wip · CoderBotOrg/backend-legacy · 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
import camera_sensors
import camera
import sensors
import movements
import sound
from utils.POO import SingletonDecorator as Singleton
from config import MultifileConfig as Config


@Singleton
class CoderBot(object):
    def __init__(self):
        # Avoid reinitialisation in case of multiple call
        # Theorically impossible due to the Singleton decorator
        if hasattr(self, '_init_complete') and self._init_complete: return

        # Camera Configuration
        # Splitter ports are for :
        # 0 - Capture full size images (default resolution)
        # 1 - Recording full size video (without opencv processing drawn)
        # 2 - Image processing for the first resolution
        # 3 - Image processing for the second resolution
        resolutions = Config().get('camera_resolutions', {'LD': (160,120), 'SD':(320,240), 'default':(1024,768)})
        self.camera = camera.Camera(resolutions['default'], Config().get('camera_framerate', 30))
        self.streamers = {}
        definitions = [r for r in resolutions.keys() if r <> 'default']
        for i, definition in enumerate(definitions, start=2):
            self.streamers[definition] = self.camera.getGrabber(threads=4, size=resolutions[definition], port=i)

        # Camera's Sensors Configuration
        self.sensors = {}
        for sensor, klass in camera_sensors.sensors.iteritems():
            self.sensors[sensor] = klass(self.streamers['LD'], draw=self.streamers['SD'])
        self.sensors['color'].setColor((51,153,153))
        self.sensors['color'].setColor((162,161,105))
        #self.sensors['light']._start()
        self.sensors['fps']._start()

        # Motors Configuration
        if Config().get('use_servos', True):
            if Config().get('use_camera_assisted_moves', True):
                self.motors = movements.ServosMotionControlled(self.sensors['flow'],
                    Config().get('pin_servo_left', 25), Config().get('pin_servo_right', 4),
                    Config().get('camera_assisted_sensibility', 0.5))
            else:
                self.motors = movements.ServosControl(Config().get('pin_servo_left', 25), Config().get('pin_servo_right', 4))
        else:
            if Config().get('use_camera_assisted_moves', True):
                self.motors = movements.MotorsMotionControlled(self.sensors['flow'], Config().get('pin_enable_motor', 22),
                    Config().get('pin_motor_left_forward', 25), Config().get('pin_motor_left_backward', 24),
                    Config().get('pin_motor_right_forward', 4), Config().get('pin_motor_right_backward', 17),
                    Config().get('camera_assisted_sensibility', 0.5))
            else:
                self.motors = movements.MotorsControl(Config().get('pin_enable_motor', 22),
                    Config().get('pin_motor_left_forward', 25), Config().get('pin_motor_left_backward', 24),
                    Config().get('pin_motor_right_forward', 4), Config().get('pin_motor_right_backward', 17))
            self.motors.freq(Config().get('PWM_frequency', 100))

        # Sound Configuration
        self.sound = sound.Sound()
        if Config().get('useMbrola', False): self.sound.useMbrola()

        # Initialisation of superclass is complete
        self._init_complete = True

    def shutdown(self):
        self._init_complete = False
        self.motors.stop()
        for sensor in self.sensors.itervalues():
            try: sensor._stop()
            except AttributeError: pass
        self.camera.close()

    def run_demo(self, during=60):
        import time, sys, cv2
        from utils.cv import faceDetect

        faces = []
        self.streamers['LD'].addProcess(faceDetect)

        start = time.time()
        while time.time() - start < during:
            if hasattr(self.streamers['LD'].frame, 'faces'): faces = self.streamers['LD'].frame.faces
            if len(faces):
                (x,y,w,h) = faces[0]
                if x+w/2 < 40:
                    print '\rleft  ',
                    self.motors.left(25)
                elif x+w/2 > 120:
                    print '\rright ',
                    self.motors.right(25)
                else:
                    print '\rstop  ',
                    self.motors.stop()
            else:
                print '\rsearch',
                self.motors.left(25)
            sys.stdout.flush()
            time.sleep(0.05)

if __name__ == '__main__':
    bot = CoderBot()
    bot.sound.useMbrola()
    bot.sound.language('fr')
    try:
        bot.run_demo()
    except KeyboardInterrupt:
        pass
    except:
        raise
    finally:
        bot.shutdown()