diff --git a/pykick/__main__.py b/pykick/__main__.py index 2bb559c..850d225 100644 --- a/pykick/__main__.py +++ b/pykick/__main__.py @@ -116,7 +116,22 @@ if __name__ == '__main__': if approach_steps < 2: state = 'ball_approach' else: + state = 'straight_approach' + + elif state == 'straight_approach': + bil = striker.get_ball_angles_from_camera( + striker.lower_camera + ) # Ball in lower + print(bil) + if bil is not None and bil[1] > 0.15: + striker.speak('Ball is close enough, stop approach') + striker.mover.stop_moving() + striker.speak('Align to goal') state = 'goal_align' + else: + striker.speak('Continue running') + striker.run_to_ball(1) + state = 'tracking' elif state == 'ball_approach': bil = striker.get_ball_angles_from_camera( @@ -127,8 +142,9 @@ if __name__ == '__main__': d = striker.distance_to_ball() except ValueError: if bil is not None: - state = 'goal_align' - striker.speak('Ball is close. Align to. Goal') + state = 'straight_approach' + striker.speak('Ball is close. ' + + 'But not close enough maybe') else: state = 'tracking' continue @@ -143,6 +159,7 @@ if __name__ == '__main__': striker.mover.move_to(0, 0, angle) striker.mover.wait() striker.run_to_ball(d_run) + striker.mover.wait() striker.speak("I think I have reached the ball. " + "I will start rotating") approach_steps += 1 diff --git a/pykick/imagereaders.py b/pykick/imagereaders.py index f4ddfbc..5d91de3 100644 --- a/pykick/imagereaders.py +++ b/pykick/imagereaders.py @@ -58,13 +58,16 @@ class NaoImageReader(object): def close(self): self.vd.unsubscribe(self.sub) print(self.sub + 'captured %s frames' % len(self.recording)) + print('Writing to', self.video_file) if self.video_file is not None: vf = cv2.VideoWriter(self.video_file, cv2.cv.FOURCC('X', 'V', 'I', 'D'), self.fps, (self.res[1], self.res[0])) for frame in self.recording: + print('.') vf.write(frame) + vf.release() def restart(self): self.vd.unsubscribe(self.sub) diff --git a/pykick/striker.py b/pykick/striker.py index b42ac27..8596f62 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -35,12 +35,12 @@ class Striker(object): # POV self.upper_pov = NaoImageReader( nao_ip, port=nao_port, res=0, fps=15, cam_id=0, - video_file='cam0_' + self.run_id + '.mpg' + video_file='./cam0_' + self.run_id + '.avi' ) self.lower_pov = NaoImageReader( nao_ip, port=nao_port, res=0, fps=15, cam_id=1, - video_file='cam1_' + self.run_id + '.mpg' + video_file='./cam1_' + self.run_id + '.avi' ) self.pov_thread = Thread(target=self._pov) self.pov_thread.start() @@ -217,7 +217,7 @@ class Striker(object): def run_to_ball(self, d): self.mover.move_to(d, 0, 0) - self.mover.wait() + # self.mover.wait() def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0): """Align robot to the ball.