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 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_y_goal(self, size): image = get_image_array(self.y_goal) image_resized = np.array(Image.fromarray(image).resize(size)) return UncertainImage(image_resized)