Better video saving + successful goal

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

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)

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.