diff --git a/pykick/imagereaders.py b/pykick/imagereaders.py index 4b0ca25..fcc83cd 100644 --- a/pykick/imagereaders.py +++ b/pykick/imagereaders.py @@ -21,8 +21,9 @@ class NaoImageReader(object): self.res = self.RESOLUTIONS[res] self.cam_id=cam_id self.vd = ALProxy('ALVideoDevice', ip, port) + streamer_name = '_'.join(['lower' if cam_id else 'upper', str(res)]) self.sub = self.vd.subscribeCamera( - "video_streamer", cam_id, res, 13, fps + streamer_name, cam_id, res, 13, fps ) def to_angles(self, x, y): @@ -36,10 +37,8 @@ class NaoImageReader(object): def get_frame(self): result = self.vd.getImageRemote(self.sub) self.vd.releaseImage(self.sub) - if result == None: - raise RuntimeError('cannot capture') - elif result[6] == None: - raise ValueError('no image data string') + if result is None or result[6] is None: + raise RuntimeError("Couldn't capture") else: height, width = self.res return np.frombuffer(result[6], dtype=np.uint8).reshape( diff --git a/pykick/striker.py b/pykick/striker.py index b78cb31..8fed0dc 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -30,23 +30,24 @@ class Striker(object): self.loss_counter = 0 self.run_after = run_after self.in_move = False - self.speaker = ALProxy("ALTextToSpeech",bytes(nao_ip),nao_port) - self.speaker.setParameter("pitchShift",2) - self.speaker.setParameter("speed",50) + self.speaker = ALProxy("ALTextToSpeech", bytes(nao_ip), nao_port) + self.speaker.setParameter("pitchShift", 2) + self.speaker.setParameter("speed", 50) self.tts_thread = None self.last_speak = None def speak(self, text): - if (self.tts_thread is None or not self.tts_thread.isAlive() ) and text!=self.last_speak: + if ( + (self.tts_thread is None or not self.tts_thread.isAlive()) + and text != self.last_speak + ): self.tts_thread = Thread( target=lambda text: self.speaker.say(str(text)), args=(text,) ) self.tts_thread.start() - self.last_speak=text - - - + self.last_speak = text + def ball_scan(self): """Intelligently rotate the robot to search for stuff.""" yaw = self.mover.get_head_angles()[0] @@ -63,12 +64,17 @@ class Striker(object): # rotate head to the left, if head yaw angle is equally zero or larger # rotate head to the right, if head yaw angle is smaller than zero else: - #self.speak("I have found the ball") + # self.speak("I have found the ball") self.mover.change_head_angles(sign * pi / 4, 0, 0.5) def get_ball_angles_from_camera(self, cam): """Detect the ball and return its angles in camera coordinates.""" - ball = self.ball_finder.find(cam.get_frame()) + try: + ball = self.ball_finder.find(cam.get_frame()) + except RuntimeError as e: + print(e) + return None + if ball is None: return None @@ -112,7 +118,7 @@ class Striker(object): # turn to ball, if the angle between the ball and the robot is too big if abs(x) > 0.15: - #self.speak('Align to the ball') + # self.speak('Align to the ball') self.mover.stop_moving() self.turn_to_ball(x, y) return False @@ -153,7 +159,6 @@ class Striker(object): self.mover.move_to(0, 0, yaw) self.mover.wait() - def align_to_ball(self): ball_angles = self.get_ball_angles_from_camera(self.lower_camera) if ball_angles is None: @@ -302,19 +307,18 @@ if __name__ == '__main__': striker.mover.kick() striker.mover.rest() - # perform normal state-machine if no input argument is given # (see diagram above) else: try: # Hit Ctrl-C to stop, cleanup and exit state = 'tracking' - #t = None + # t = None while True: # meassure time for debbuging loop_start = time() print('State:', state) - #striker.speak(str(state)) - + # striker.speak(str(state)) + if state == 'tracking': # start ball approach when ball is visible if striker.ball_tracking(): @@ -327,28 +331,26 @@ if __name__ == '__main__': ) print(ball_in_lower) if (ball_in_lower is not None and ball_in_lower[1] > 0.28): - - #t = Thread(target=striker.speak, args=["Ball is close enough, stop approach"]) - #t.start() + print('Ball is close enough, stop approach') - #striker.speak("Ball is close enough stop approach") - #striker.mover.stop_moving() - #state = 'align' - state='simple_kick' + # striker.speak("Ball is close enough stop approach") + # striker.mover.stop_moving() + # state = 'align' + state = 'simple_kick' else: print('Continue running') - ##striker.speak("Continue running") + # striker.speak("Continue running") striker.run_to_ball() state = 'tracking' elif state == 'simple_kick': - #striker.mover.set_head_angles(0,0.25,0.3) + # striker.mover.set_head_angles(0,0.25,0.3) print('Doing the simple kick') # just walk a short distance forward, ball should be near # and it will probably be kicked in the right direction striker.speak("Simple Kick") - striker.mover.move_to(0.3,0,0) + striker.mover.move_to(0.3, 0, 0) striker.mover.wait() state = 'tracking'