diff --git a/pykick/__main__.py b/pykick/__main__.py index 11e4c69..8b02faa 100644 --- a/pykick/__main__.py +++ b/pykick/__main__.py @@ -66,6 +66,8 @@ if __name__ == '__main__': goal_hsv=cfg['goal'], field_hsv=cfg['field'], ball_min_radius=cfg['ball_min_radius'], ) + print('Initialized') + striker.mover.stand_up() state = 'init' init_soll = 0.0 diff --git a/pykick/imagereaders.py b/pykick/imagereaders.py index ab6452b..f4ddfbc 100644 --- a/pykick/imagereaders.py +++ b/pykick/imagereaders.py @@ -11,6 +11,7 @@ except: class NaoImageReader(object): RESOLUTIONS = { + 0: (120, 160), 1: (240, 320), 2: (480, 640), 3: (960, 1280) @@ -26,7 +27,6 @@ class NaoImageReader(object): self.fps = fps self.vd = ALProxy('ALVideoDevice', ip, port) streamer_name = '_'.join(['lower' if cam_id else 'upper', str(res)]) - print(streamer_name) self.sub = self.vd.subscribeCamera( streamer_name, cam_id, res, 13, fps ) @@ -57,10 +57,11 @@ class NaoImageReader(object): def close(self): self.vd.unsubscribe(self.sub) + print(self.sub + 'captured %s frames' % len(self.recording)) if self.video_file is not None: vf = cv2.VideoWriter(self.video_file, cv2.cv.FOURCC('X', 'V', 'I', 'D'), - 5, + self.fps, (self.res[1], self.res[0])) for frame in self.recording: vf.write(frame) @@ -70,6 +71,7 @@ class NaoImageReader(object): self.sub = self.vd.subscribeCamera( self.sub, self.cam_id, self.res_id, 13, self.fps ) + self.recording = [] class VideoReader(object): diff --git a/pykick/striker.py b/pykick/striker.py index eff2900..1faa204 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -16,36 +16,66 @@ class Striker(object): def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv, ball_min_radius): + + # Timestamp self.run_id = strftime('%Y%m%d%H%M%S') + + # Motion self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port) - self.mover.stand_up() + self.is_over = False + + # Sight self.upper_camera = NaoImageReader( nao_ip, port=nao_port, res=res, fps=30, cam_id=0, - video_file='cam0_' + self.run_id + '.mpg' ) self.lower_camera = NaoImageReader( nao_ip, port=nao_port, res=res, fps=30, cam_id=1, + ) + + # POV + self.upper_pov = NaoImageReader( + nao_ip, port=nao_port, res=0, fps=15, cam_id=0, + video_file='cam0_' + self.run_id + '.mpg' + ) + + self.lower_pov = NaoImageReader( + nao_ip, port=nao_port, res=0, fps=15, cam_id=1, video_file='cam1_' + self.run_id + '.mpg' ) + self.pov_thread = Thread(target=self._pov) + self.pov_thread.start() + + # Recognition self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]), ball_min_radius) self.field_finder = FieldFinder(tuple(field_hsv[0]), tuple(field_hsv[1])) self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1])) + + # Talking self.speaker = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port) - - self.is_over = False - self.speach_queue = deque(maxlen=4) self.speach_history = [] self.tts_thread = Thread(target=self._speaker) self.tts_thread.start() - self.rotating = False - self.rot_dir = 0 - self.timer_start = 0 - self.timer_stop = 0 - self.dy = False + # Debugging + self._timer_start = 0 + self._timer_stop = 0 + + def close(self): + self.is_over = True + + if self.tts_thread.isAlive(): + self.tts_thread.join() + if self.pov_thread.isAlive(): + self.pov_thread.join() + + self.upper_camera.close() + self.lower_camera.close() + self.upper_pov.close() + self.lower_pov.close() + self.mover.stop_moving() def _speaker(self): while not self.is_over: @@ -53,6 +83,15 @@ class Striker(object): self.speaker.say(self.speach_queue.pop()) sleep(0.1) + def _pov(self): + while not self.is_over: + try: + self.upper_pov.get_frame() + self.lower_pov.get_frame() + except RuntimeError as e: + print(e) + continue + def speak(self, text): if not self.speach_history or self.speach_history[-1] != text: self.speach_queue.appendleft(text) @@ -159,7 +198,7 @@ class Striker(object): ball_angles = self.get_ball_angles_from_camera(cam) if ball_angles is not None: x, y = ball_angles - self.timer_start = time() + self._timer_start = time() in_sight = True break @@ -201,8 +240,8 @@ class Striker(object): sleep(0.1) head_yaw, head_pitch = self.mover.get_head_angles() - self.timer_stop = time() - print('Ball to head', self.timer_stop - self.timer_start) + self._timer_stop = time() + print('Ball to head', self._timer_stop - self._timer_start) print('Head yaw', head_yaw, end=' ') d_yaw = head_yaw - soll print('Head d_yaw', d_yaw) @@ -305,15 +344,6 @@ class Striker(object): self.mover.wait() return False - def close(self): - self.is_over = True - if self.tts_thread.isAlive(): - self.tts_thread.join() - self.upper_camera.close() - self.lower_camera.close() - self.mover.stop_moving() - # self.mover.stand_up() - def goal_search(self): goal_center_x = None positions = [0, pi/6, pi/4, pi/3, pi/2]