def preprocess(bagfile, output, output_size):
""" yields topic, data, t but data already a numpy array """
bag = rosbag.Bag(bagfile)
info = yaml.load(bag._get_yaml_info())
for topic_dict in info['topics']:
if 'FloatArray' in topic_dict['type']:
command_topic = topic_dict['topic']
topics = [topic_image_raw, command_topic]
logger.info('bag_processing, using topics: %s' % topics)
def read_tuples(raw_stream):
i = 0
Y0 = None
U0 = None
Y_last = None
t0 = 0
for topic, msg, t in raw_stream:
# logger.debug('read msg type %s and topic %s' % (msg._type, topic))
i += 1
if topic == command_topic:
# logger.debug('command read')
Y1 = Y_last
if Y0 is not None and U0 is not None:
# yeild tuple
yield (Y0, U0, Y1, t0)
Y0 = Y_last
U0 = msg
t0 = t
if topic == topic_image_raw:
# logger.debug('image read')
Y_last = msg
tuples_stream = read_tuples(bag.read_messages(topics=topics))
out_bag = rosbag.Bag(output, 'w')
for Y0, U, Y1, t0 in tuples_stream:
y0_pil = Image.fromarray(get_image_array(Y0)).resize(output_size)
y1_pil = Image.fromarray(get_image_array(Y1)).resize(output_size)
# pdb.set_trace()
logger.info('Writing data (Y0, U0=%s, Y1)' % U)
out_bag.write('Y0', pil_to_imgmsg(y0_pil), t0)
out_bag.write('U0', U, t0)
out_bag.write('Y1', pil_to_imgmsg(y1_pil), t0)
out_bag.close()
def find_dt_threshold(bagfile, stream, distance, ignore_first=100, max_images=300):
"""
Finds the value of dy/dt for when the robot is probably moving
:param infile: File to read data from
"""
bag = rosbag.Bag(bagfile)
logger.info("Calibrating diff_threshold for camera motion...")
ic = 0
Y_last = None
diffs = []
for _, msg, _ in bag.read_messages(stream):
Y = get_image_array(msg)
ic = ic + 1
if ic > max_images:
print("break")
break
if ic < ignore_first:
continue
if Y_last is not None:
diff = distance(Y_last, Y)
diffs.append(diff)
Y_last = Y
bag.close()
diff_threshold = 2 * np.mean(diffs)
logger.info("diff_threshold = %g " % diff_threshold)
return diff_threshold
def show_images(self):
# Initiate services to get images
get_goal_service = rospy.ServiceProxy(self.server_name + '/get_goal',
camera_actuator.srv.imageService)
get_current_service = rospy.ServiceProxy(self.server_name + '/get_current',
camera_actuator.srv.imageService)
im_goal = get_goal_service(0)
im_current = get_current_service(0)
np_goal = get_image_array(im_goal.image)
pil_goal = Image.fromarray(np_goal)
np_current = get_image_array(im_current.image)
pil_current = Image.fromarray(np_current)
self.ui = UI(pil_goal, pil_current)
def find_dt_threshold(bagfile, stream, distance, ignore_first=100, max_images=300):
'''
Finds the value of dy/dt for when the robot is probably moving
:param infile: File to read data from
'''
bag = rosbag.Bag(bagfile)
logger.info('Calibrating diff_threshold for camera motion...')
ic = 0
Y_last = None
diffs = []
for _, msg, _ in bag.read_messages(stream):
Y = get_image_array(msg)
ic = ic + 1
if ic > max_images:
print('break')
break
if ic < ignore_first:
continue
if Y_last is not None:
diff = distance(Y_last, Y)
diffs.append(diff)
Y_last = Y
bag.close()
diff_threshold = 2 * np.mean(diffs)
logger.info('diff_threshold = %g ' % diff_threshold)
return diff_threshold
def show_images(self):
# Initiate services to get images
get_goal_service = rospy.ServiceProxy(self.server_name + '/get_goal',
camera_actuator.srv.imageService)
get_current_service = rospy.ServiceProxy(
self.server_name + '/get_current',
camera_actuator.srv.imageService)
im_goal = get_goal_service(0)
im_current = get_current_service(0)
np_goal = get_image_array(im_goal.image)
pil_goal = Image.fromarray(np_goal)
np_current = get_image_array(im_current.image)
pil_current = Image.fromarray(np_current)
self.ui = UI(pil_goal, pil_current)
def run_demo(self, n):
# args = line.split(' ')
# if len(args) <= 1:
# n = 10
# else:
# n = args[1]
pylab.figure()
iter_results = []
for i in range(n):
print('run_one ' + str(i))
# pdb.set_trace()
y_0 = get_image_array(self.y_current)
plan, plan_executed = self.run_one()
if plan == (-1, ):
break
dist0 = self.get_distance(0).res
pred_dist = self.get_p_d(plan).array
self.set_current()
dist = self.get_distance(0).res
iter_result = {
'i': i,
'plan_found': tuple(plan),
'plan_executed': plan_executed,
'start_distance': dist0,
'final_distance': dist,
'predicted_distances': pred_dist,
'y_0': y_0,
'y_1': get_image_array(self.y_current)
}
iter_results.append(iter_result)
image = self.update_distance_plot(iter_result)
# ros_img = pil_to_imgmsg(image)
# ros_img = numpy_to_imgmsg(np.array(image))
# self.distance_pub.publish(ros_img)
run_result = {'iter_results': iter_results}
return run_result
def run_demo(self, n):
# args = line.split(' ')
# if len(args) <= 1:
# n = 10
# else:
# n = args[1]
pylab.figure()
iter_results = []
for i in range(n):
print('run_one ' + str(i))
# pdb.set_trace()
y_0 = get_image_array(self.y_current)
plan, plan_executed = self.run_one()
if plan == (-1,):
break
dist0 = self.get_distance(0).res
pred_dist = self.get_p_d(plan).array
self.set_current()
dist = self.get_distance(0).res
iter_result = {'i':i,
'plan_found':tuple(plan),
'plan_executed':plan_executed,
'start_distance':dist0,
'final_distance':dist,
'predicted_distances':pred_dist,
'y_0':y_0,
'y_1':get_image_array(self.y_current)}
iter_results.append(iter_result)
image = self.update_distance_plot(iter_result)
# ros_img = pil_to_imgmsg(image)
# ros_img = numpy_to_imgmsg(np.array(image))
# self.distance_pub.publish(ros_img)
run_result = {'iter_results':iter_results}
return run_result
def received_image(self, t, imgmsg):
# first thing, convert to numpy
image = get_image_array(imgmsg)
if self.use_zoom:
processed = zoom_image_center(image, self.current_zoom, self.output_size)
else:
processed = resize(image,
height=self.output_size[1],
width=self.output_size[0])
self.queue_image.append((t, processed))
def received_image(self, t, imgmsg):
# first thing, convert to numpy
image = get_image_array(imgmsg)
if self.use_zoom:
processed = zoom_image_center(image, self.current_zoom,
self.output_size)
else:
processed = resize(image,
height=self.output_size[1],
width=self.output_size[0])
self.queue_image.append((t, processed))
def run_demo(self, n):
iter_results = []
for i in range(n):
print('run_one ' + str(i))
# pdb.set_trace()
y_0 = get_image_array(self.y_current)
plan, plan_executed = self.run_one()
if plan == (-1,):
break
self.set_current()
iter_result = {'i':i,
'plan_found':tuple(plan),
'plan_executed':plan_executed,
'y_0':y_0,
'y_1':get_image_array(self.y_current)}
iter_results.append(iter_result)
run_result = {'iter_results':iter_results}
return run_result
def run_demo(self, n):
iter_results = []
for i in range(n):
print('run_one ' + str(i))
# pdb.set_trace()
y_0 = get_image_array(self.y_current)
plan, plan_executed = self.run_one()
if plan == (-1, ):
break
self.set_current()
iter_result = {
'i': i,
'plan_found': tuple(plan),
'plan_executed': plan_executed,
'y_0': y_0,
'y_1': get_image_array(self.y_current)
}
iter_results.append(iter_result)
run_result = {'iter_results': iter_results}
return run_result
def get_image(self):
'''
Requests an image from the ROS service specified by IMAGE_SERVICE.
Resize to desired size and returns as an UncertainImage
:param size: size of the image
:return image_resized: as an UncertainImage
'''
# Connection to ROS service
takeImage = rospy.ServiceProxy(IMAGE_SERVICE, camera_actuator.srv.imageService)
# Get image and convert to numpy
image = get_image_array(takeImage(0).image)
size = self.size
# Convert to PIL, resize and back to numpy
image_resized = np.array(Image.fromarray(image).resize((size[1], size[0])))
# Return the resized image as UncertainImage
return UncertainImage(image_resized)
def get_image(self):
'''
Requests an image from the ROS service specified by IMAGE_SERVICE.
Resize to desired size and returns as an UncertainImage
:param size: size of the image
:return image_resized: as an UncertainImage
'''
# Connection to ROS service
takeImage = rospy.ServiceProxy(IMAGE_SERVICE,
camera_actuator.srv.imageService)
# Get image and convert to numpy
image = get_image_array(takeImage(0).image)
size = self.size
# Convert to PIL, resize and back to numpy
image_resized = np.array(
Image.fromarray(image).resize((size[1], size[0])))
# Return the resized image as UncertainImage
return UncertainImage(image_resized)
def get_uncertain_image(image, size):
image = get_image_array(image)
image_resized = np.array(Image.fromarray(image).resize(size))
return UncertainImage(image_resized)
def get_uncertain_image(image, size):
image = get_image_array(image)
image_resized = np.array(Image.fromarray(image).resize(size))
return UncertainImage(image_resized)
def get_y_goal(self, size):
image = get_image_array(self.y_goal)
image_resized = np.array(Image.fromarray(image).resize(size))
return UncertainImage(image_resized)
