pavel/kick-it
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fixed RuntimeError('Cannot capture')

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-23 13:29:10 +02:00
parent 126329bafd
commit 4536a4d1ac
2 changed files with 32 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
pykick/imagereaders.py
View File

@@ -21,8 +21,9 @@ class NaoImageReader(object):
self.res = self.RESOLUTIONS[res] self.res = self.RESOLUTIONS[res]
self.cam_id=cam_id self.cam_id=cam_id
self.vd = ALProxy('ALVideoDevice', ip, port) self.vd = ALProxy('ALVideoDevice', ip, port)
streamer_name = '_'.join(['lower' if cam_id else 'upper', str(res)])
self.sub = self.vd.subscribeCamera( 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): def to_angles(self, x, y):
@@ -36,10 +37,8 @@ class NaoImageReader(object):
def get_frame(self): def get_frame(self):
result = self.vd.getImageRemote(self.sub) result = self.vd.getImageRemote(self.sub)
self.vd.releaseImage(self.sub) self.vd.releaseImage(self.sub)
if result == None: if result is None or result[6] is None:
raise RuntimeError('cannot capture') raise RuntimeError("Couldn't capture")
elif result[6] == None:
raise ValueError('no image data string')
else: else:
height, width = self.res height, width = self.res
return np.frombuffer(result[6], dtype=np.uint8).reshape( return np.frombuffer(result[6], dtype=np.uint8).reshape(

54
pykick/striker.py
View File

@@ -30,23 +30,24 @@ class Striker(object):
self.loss_counter = 0 self.loss_counter = 0
self.run_after = run_after self.run_after = run_after
self.in_move = False self.in_move = False
self.speaker = ALProxy("ALTextToSpeech",bytes(nao_ip),nao_port) self.speaker = ALProxy("ALTextToSpeech", bytes(nao_ip), nao_port)
self.speaker.setParameter("pitchShift",2) self.speaker.setParameter("pitchShift", 2)
self.speaker.setParameter("speed",50) self.speaker.setParameter("speed", 50)
self.tts_thread = None self.tts_thread = None
self.last_speak = None self.last_speak = None
def speak(self, text): 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( self.tts_thread = Thread(
target=lambda text: self.speaker.say(str(text)), target=lambda text: self.speaker.say(str(text)),
args=(text,) args=(text,)
) )
self.tts_thread.start() self.tts_thread.start()
self.last_speak=text self.last_speak = text
def ball_scan(self): def ball_scan(self):
"""Intelligently rotate the robot to search for stuff.""" """Intelligently rotate the robot to search for stuff."""
yaw = self.mover.get_head_angles()[0] 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 left, if head yaw angle is equally zero or larger
# rotate head to the right, if head yaw angle is smaller than zero # rotate head to the right, if head yaw angle is smaller than zero
else: 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) self.mover.change_head_angles(sign * pi / 4, 0, 0.5)
def get_ball_angles_from_camera(self, cam): def get_ball_angles_from_camera(self, cam):
"""Detect the ball and return its angles in camera coordinates.""" """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: if ball is None:
return 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 # turn to ball, if the angle between the ball and the robot is too big
if abs(x) > 0.15: if abs(x) > 0.15:
#self.speak('Align to the ball') # self.speak('Align to the ball')
self.mover.stop_moving() self.mover.stop_moving()
self.turn_to_ball(x, y) self.turn_to_ball(x, y)
return False return False
@@ -153,7 +159,6 @@ class Striker(object):
self.mover.move_to(0, 0, yaw) self.mover.move_to(0, 0, yaw)
self.mover.wait() self.mover.wait()
def align_to_ball(self): def align_to_ball(self):
ball_angles = self.get_ball_angles_from_camera(self.lower_camera) ball_angles = self.get_ball_angles_from_camera(self.lower_camera)
if ball_angles is None: if ball_angles is None:
@@ -302,19 +307,18 @@ if __name__ == '__main__':
striker.mover.kick() striker.mover.kick()
striker.mover.rest() striker.mover.rest()
# perform normal state-machine if no input argument is given # perform normal state-machine if no input argument is given
# (see diagram above) # (see diagram above)
else: else:
try: # Hit Ctrl-C to stop, cleanup and exit try: # Hit Ctrl-C to stop, cleanup and exit
state = 'tracking' state = 'tracking'
#t = None # t = None
while True: while True:
# meassure time for debbuging # meassure time for debbuging
loop_start = time() loop_start = time()
print('State:', state) print('State:', state)
#striker.speak(str(state)) # striker.speak(str(state))
if state == 'tracking': if state == 'tracking':
# start ball approach when ball is visible # start ball approach when ball is visible
if striker.ball_tracking(): if striker.ball_tracking():
@@ -327,28 +331,26 @@ if __name__ == '__main__':
) )
print(ball_in_lower) print(ball_in_lower)
if (ball_in_lower is not None and ball_in_lower[1] > 0.28): 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') print('Ball is close enough, stop approach')
#striker.speak("Ball is close enough stop approach") # striker.speak("Ball is close enough stop approach")
#striker.mover.stop_moving() # striker.mover.stop_moving()
#state = 'align' # state = 'align'
state='simple_kick' state = 'simple_kick'
else: else:
print('Continue running') print('Continue running')
##striker.speak("Continue running") # striker.speak("Continue running")
striker.run_to_ball() striker.run_to_ball()
state = 'tracking' state = 'tracking'
elif state == 'simple_kick': 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') print('Doing the simple kick')
# just walk a short distance forward, ball should be near # just walk a short distance forward, ball should be near
# and it will probably be kicked in the right direction # and it will probably be kicked in the right direction
striker.speak("Simple Kick") striker.speak("Simple Kick")
striker.mover.move_to(0.3,0,0) striker.mover.move_to(0.3, 0, 0)
striker.mover.wait() striker.mover.wait()
state = 'tracking' state = 'tracking'