fixed RuntimeError('Cannot capture')
This commit is contained in:
@@ -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(
|
||||
|
||||
@@ -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'
|
||||
|
||||
|
||||
Reference in New Issue
Block a user