def get_image(self):
msg = rospy.client.wait_for_message(ns+'image_raw', Image)
h = msg.height
w = msg.width
# Should probably raise a ROS exception of some sort here
if (w, h) != (856, 480):
self.logwarn('WARNING: Unexpected image size: {} x {}'.format(w, h))
s = 4 # TODO: force expected size instead
if self.display and self.iw is None:
self.iw = ImageWindow(w/2, h/2)
self._count = 0
self.loginfo('{}: Got {} x {} image'.format(self._count, w, h))
self._count += 1
# Crop out the middle of the image
# TODO: Tighten up the math for the processing loop
image = PILImage.frombytes('RGB', (w, h), msg.data)\
.crop((w/s, h/s, (s-1)*w/s, (s-1)*h/s))
resized = image.resize((w/s, h/s), resample=PILImage.LANCZOS)
if self.display:
self.iw.show_image(image).update()
# Convert to HSV color space
hsv = resized.convert('HSV')
# Return properly-shaped raw data
return np.fromstring(hsv.tobytes(), dtype='byte')\
.reshape((h/s, w/s, 3))
def play(self, file, mode='HSV'):
npz = np.load(file)
data = self.data = npz['data']
labels = self.labels = npz['labels']
n = self.num_annotated = data.shape[0]
w = self.width = 214
h = self.height = 120
c = self.chans = 3
size = w * h * c
iw = ImageWindow(w, h)
assert size == data[0].size, "Unexpected buffer size (%d vs %d)" % (
size, data[0].size)
assert data.shape[0] == labels.shape[
0], "Mismatched image and label count"
i = 0
while i < n:
image = Image.frombytes(mode, (w, h), data[i])
iw.show_image(image)
iw.force_focus()
print('Image {} / {}: {}'.format(i, n, Action.name(labels[i])))
iw.wait()
key = iw.get_key()
if key == 'Escape':
break
elif key == 'BackSpace':
if i > 0:
i -= 1
continue
else:
i += 1
def simulate(self, file):
# Load jpeg-encoded images from rosbag file
self._load_bag_data(file)
# This logic belongs elsewhere
# Initialize display window
if self.display:
self.iw = ImageWindow(self.width, self.height)
# Initialize ROS node
rospy.init_node('camera_simulator')
# Begin 4 Hz loop
rate = rospy.Rate(4)
while not rospy.is_shutdown():
img_msg = self.make_image_msg(self.next_image())
self.log_image(img_msg)
self.camera.publish(img_msg)
rate.sleep()
def reannotate(self, bagfile, npzfile_in):
self._load_bag_data(bagfile) # self.num_images set here
w = self.width
h = self.height
s = self.scale
c = self.chans
size = w*h/s/s*c
iw = ImageWindow(w/2, h/2)
edit = False
npz = np.load(npzfile_in)
labels = self.labels = npz['labels']
n = self.num_annotated = labels.shape[0]
if self.num_images != self.num_annotated:
print('Warning: bag and npz file lengths differ ({} vs {})'.format(self.num_images, self.num_annotated))
data = self.data = np.empty((self.num_annotated, size), dtype='byte')
# Check that our incoming image size is as expected...
image = Image.open(BytesIO(self.image_data[0]))
resized = image.resize((w/s, h/s), resample=Image.LANCZOS)
hsv = resized.convert('HSV')
assert size == np.fromstring(hsv.tobytes(), dtype='byte').size, "Unexpected image size!"
i = 0
while i < self.num_annotated:
image = Image.open(BytesIO(self.image_data[i]))\
.crop((w/s, h/s, (s-1)*w/s, (s-1)*h/s))
resized = image.resize((w/s, h/s), resample=Image.LANCZOS)
hsv = resized.convert('HSV')
iw.show_image(image)
iw.force_focus()
if edit:
print('Image {} / {} ({}): '.format(i, self.num_annotated, Action.name(labels[i])), end='')
sys.stdout.flush()
else:
print('Image {} / {}: {}'.format(i, n, Action.name(labels[i])))
if labels[i] >= self.num_actions:
print ('>>> CHANGING TO {}'.format(Action.name(Action.SCAN)))
iw.wait()
key = iw.get_key()
if key=='Escape':
print('(QUIT)')
break
elif key=='BackSpace':
if i > 0:
i -= 1
print('(BACK)')
continue
elif key=='e':
if edit:
edit = False
print('(EDIT OFF)')
else:
edit = True
print('(EDIT ON)')
continue
elif not edit:
if labels[i] >= self.num_actions:
label = Action.SCAN
else:
label = self.labels[i]
elif key=='space':
label = Action.SCAN
elif key=='Return':
label = Action.TARGET
elif self.num_actions > 2 and key=='Left':
label = Action.TARGET_LEFT
elif self.num_actions > 2 and key=='Right':
label = Action.TARGET_RIGHT
elif self.num_actions > 4 and key=='Up':
label = Action.TARGET_UP
elif self.num_actions > 4 and key=='Down':
label = Action.TARGET_DOWN
else:
label = Action.SCAN
self.labels[i] = label
self.data[i] = np.fromstring(hsv.tobytes(), dtype='byte')
if edit:
print(Action.name(label))
i += 1
iw.close()
self.num_annotated = i
def annotate(self, file):
self._load_bag_data(file)
w = self.width
h = self.height
s = self.scale
c = self.chans
size = w*h/s/s*c
iw = ImageWindow(w, h)
self.labels = np.empty(self.num_images, dtype='byte')
self.data = np.empty((self.num_images, size), dtype='byte')
# Check that our incoming image size is as expected...
image = Image.open(BytesIO(self.image_data[0]))
resized = image.resize((w/s, h/s), resample=Image.LANCZOS)
hsv = resized.convert('HSV')
assert size == np.fromstring(hsv.tobytes(), dtype='byte').size, "Unexpected image size!"
i = 0
while i < self.num_images:
image = Image.open(BytesIO(self.image_data[i]))
resized = image.resize((w/s, h/s), resample=Image.LANCZOS)
hsv = resized.convert('HSV')
#hue,_,_ = hsv.split()
draw = ImageDraw.Draw(image)
draw.line([(w/s, 0), (w/s, h)])
draw.line([((s-1)*w/s, 0), ((s-1)*w/s, h)])
# draw.line([(0, h/s), (w, h/s)])
# draw.line([(0, (s-1)*h/s), (w, (s-1)*h/s)])
iw.show_image(image)
iw.force_focus()
print('Image {} / {}:'.format(i, self.num_images), end='')
iw.wait()
key = iw.get_key()
if key=='Escape':
print('(QUIT)')
break
elif key=='BackSpace':
if i > 0:
i -= 1
print('(BACK)')
continue
elif key=='space':
label = Action.SCAN
elif key=='Return':
label = Action.TARGET
elif key=='Left':
label = Action.TARGET_LEFT
elif key=='Right':
label = Action.TARGET_RIGHT
elif self.num_actions > 4 and key=='Up':
label = Action.TARGET_UP
elif self.num_actions > 4 and key=='Down':
label = Action.TARGET_DOWN
else:
label = Action.SCAN
self.labels[i] = label
self.data[i] = np.fromstring(hsv.tobytes(), dtype='byte')
print(Action.name(label))
i += 1
iw.close()
self.num_annotated = i
def annotate(self, file):
self._load_data(file)
w = self.width
h = self.height
s = self.scale
c = self.chans
size = w * h / s / s * c
iw = ImageWindow(w / 2, h / 2)
self.labels = np.empty(self.num_images, dtype='byte')
self.data = np.empty((self.num_images, size), dtype='byte')
keep = np.ones(self.num_images)
# Check that our incoming image size is as expected...
image = Image.open(BytesIO(self.image_data[0]))
resized = image.resize((w / s, h / s), resample=Image.LANCZOS)
hsv = resized.convert('HSV')
assert size == np.fromstring(
hsv.tobytes(), dtype='byte').size, "Unexpected image size!"
i = 0
while i < self.num_images:
image = Image.open(BytesIO(self.image_data[i]))\
.crop((w/s, h/s, (s-1)*w/s, (s-1)*h/s))
resized = image.resize((w / s, h / s), resample=Image.LANCZOS)
hsv = resized.convert('HSV')
iw.show_image(image)
iw.force_focus()
if keep[i] == 1:
print('Image {} / {}: '.format(i, self.num_images), end='')
else:
print('Image {} / {} (KILLED): '.format(i, self.num_images),
end='')
sys.stdout.flush()
iw.wait()
key = iw.get_key()
if key == 'Escape':
print('(QUIT)')
break
elif key == 'BackSpace':
if i > 0:
i -= 1
print('(BACK)')
continue
elif key == 'k':
print('(KILLED)')
keep[i] = 0
label = Action.SCAN
elif key == 'r':
print('(RESTORED)')
keep[i] = 1
if i > 0:
i -= 1
continue
elif key == 'space':
label = Action.SCAN
elif key == 'Return':
label = Action.TARGET
elif self.num_actions > 2 and key == 'Left':
label = Action.TARGET_LEFT
elif self.num_actions > 2 and key == 'Right':
label = Action.TARGET_RIGHT
elif self.num_actions > 4 and key == 'Up':
label = Action.TARGET_UP
elif self.num_actions > 4 and key == 'Down':
label = Action.TARGET_DOWN
else:
label = Action.SCAN
self.labels[i] = label
self.data[i] = np.fromstring(hsv.tobytes(), dtype='byte')
print(Action.name(label))
i += 1
iw.close()
self.num_annotated = i
self.labels = self.labels[:i]
self.data = self.data[:i]
self.labels = self.labels[keep[:i] == 1]
self.data = self.data[keep[:i] == 1]
self.num_annotated = len(self.labels)
class CNNNavigator(object):
def __init__(self, auto=False, display=False, speak=False, caution=False, verbose=False):
self.auto = auto
self.display = display
self.speak = speak
self.iw = None
self._count = 0
self.caution = caution
self.forward = False
self.forward_time = 0
self.forward_margin = 2.0
self.verbose = verbose
if speak:
self.speaker = Speaker()
else:
self.speaker = None
self.command = Command()
# Initialize ROS node
rospy.init_node('cnn_navigator', disable_signals=True)
rospy.on_shutdown(lambda : self.emergency_land(self))
if self.auto:
# Load CNN
self.loginfo('Loading CNN model...')
self.cnn = CNNModel(verbose=False)
self.cnn.load_model()
self.loginfo('CNN model loaded.')
else:
self.cnn = None
self.flying = False
def _move_up(self, secs):
self.loginfo('Going up...')
if self.speak:
self.speaker.speak('moving up')
then = rospy.Time.now()
d = rospy.Duration.from_sec(secs)
r = rospy.Rate(10)
while rospy.Time.now() - then < d:
self.command.do('move', 'up')
r.sleep()
self.loginfo('Done.')
def takeoff(self):
self.logwarn('takeoff')
if self.speak:
self.speaker.speak("taking off")
self.command.do('takeoff')
self.flying = True
self.loginfo('Waiting 10 seconds...')
rospy.sleep(10) # Would be better to get callback when ready...
self._move_up(2.0)
def land(self):
self.loginfo('land')
if self.speak:
self.speaker.speak("landing")
self.command.do('land')
self.flying = False
def flattrim(self):
self.loginfo('flat trim')
self.command.do('flattrim')
def apply_caution(self, nav):
if not self.caution:
return nav
if nav == 'forward':
curr_time = time.time()
if self.forward:
if curr_time > self.forward_time + self.forward_margin:
# Apply the brakes
self.forward = False
nav = 'stop'
self.forward_time = curr_time
self.logwarn('Safety stop!')
if self.speak:
self.speaker.speak_new('safety stop')
else:
if curr_time > self.forward_time + self.forward_margin/2:
# ok, you can start again
self.forward = True
self.forward_time = curr_time
else:
# still in time out
self.loginfo('In time out')
if self.speak:
self.speaker.speak_new('time out')
nav = 'stop'
else:
self.forward = False
return nav
def move(self, nav):
assert nav in ['left', 'right', 'forward', 'stop']
assert self.flying
nav = self.apply_caution(nav)
self.command.do('move', nav)
self.loginfo(nav)
def stop(self):
self.move('stop')
def loginfo(self, message):
if self.verbose:
rospy.loginfo(message)
def logwarn(self, message):
rospy.loginfo(message) # There might be another method for this
def handle_uncertainty(self, c, p):
command = Action.name(c)
if c == Action.SCAN and p < 0.5:
self.logwarn('UNCERTAIN {} (p={:4.2f})'.format(command, p))
if self.speak:
self.speaker.speak_new('UNKNOWN')
c = Action.TARGET_LEFT
elif c == Action.TARGET and p < 0.5:
self.logwarn('UNCERTAIN {} (p={:4.2f})'.format(command, p))
if self.speak:
self.speaker.speak_new('UNKNOWN')
c = Action.TARGET_LEFT
return c
def navigate(self):
assert self.auto
assert self.flying
self.loginfo('Begin autonomous navigation')
try:
while True:
img = self.get_image()
# pred = self.cnn.predict_sample_class(img)
preds = self.cnn.predict_sample_proba(img)
c = np.argmax(preds)
p = preds[c]
c = self.handle_uncertainty(c, p)
self.give_command(c)
except KeyboardInterrupt:
self.loginfo('End autonomous navigation')
self.cleanup()
def watch(self):
assert self.auto
self.loginfo('Begin passive classification')
try:
while True:
img = self.get_image()
# pred = self.cnn.predict_sample_class(img)
preds = self.cnn.predict_sample_proba(img)
c = np.argmax(preds)
p = preds[c]
command = Action.name(c)
c = self.handle_uncertainty(c, p)
self.loginfo('Command {} (p={:4.2f})'.format(command, p))
self.loginfo('-----')
if self.speak:
self.speaker.speak_new(command)
except KeyboardInterrupt:
# It seems difficult to interrupt the loop when display=True
self.loginfo('End passive classification')
self.cleanup()
def give_command(self, act):
command = Action.name(act)
self.loginfo('Command {}'.format(command))
if self.speak:
self.speaker.speak_new(command)
if act in [Action.SCAN, Action.TARGET_RIGHT]:
nav = 'right'
elif act == Action.TARGET_LEFT:
nav = 'left'
elif act == Action.TARGET:
nav = 'forward'
else:
self.loginfo('Stop')
nav = 'stop'
self.move(nav)
self.loginfo('-----')
def get_image(self):
msg = rospy.client.wait_for_message(ns+'image_raw', Image)
h = msg.height
w = msg.width
# Should probably raise a ROS exception of some sort here
if (w, h) != (856, 480):
self.logwarn('WARNING: Unexpected image size: {} x {}'.format(w, h))
s = 4 # TODO: force expected size instead
if self.display and self.iw is None:
self.iw = ImageWindow(w/2, h/2)
self._count = 0
self.loginfo('{}: Got {} x {} image'.format(self._count, w, h))
self._count += 1
# Crop out the middle of the image
# TODO: Tighten up the math for the processing loop
image = PILImage.frombytes('RGB', (w, h), msg.data)\
.crop((w/s, h/s, (s-1)*w/s, (s-1)*h/s))
resized = image.resize((w/s, h/s), resample=PILImage.LANCZOS)
if self.display:
self.iw.show_image(image).update()
# Convert to HSV color space
hsv = resized.convert('HSV')
# Return properly-shaped raw data
return np.fromstring(hsv.tobytes(), dtype='byte')\
.reshape((h/s, w/s, 3))
# TODO: Other functions here?
def cleanup(self):
if self.display and self.iw is not None:
self.iw.close()
self.iw = None
def shutdown(self):
self.cleanup()
rospy.signal_shutdown('Node shutdown requested')
@staticmethod
def emergency_land(obj):
rospy.loginfo('emergency land')
obj.command.do('land')
if obj.display and obj.iw is not None:
obj.iw.close()
obj.iw = None