def estimate_homography(self,cv_image): '''Estimate ground projection using instrinsic camera calibration parameters''' cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) cv_image_rectified = self.rectify(cv_image) logger.info("image rectified") ret, corners = cv2.findChessboardCorners(cv_image_rectified, (self.board_['width'], self.board_['height'])) if ret == False: logger.error("No corners found in image") exit(1) if len(corners) != self.board_['width'] * self.board_['height']: logger.error("Not all corners found in image") exit(2) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01) corners2 = cv2.cornerSubPix(cv_image_rectified, corners, (11,11), (-1,-1), criteria) #TODO flip checks src_pts = [] for r in range(self.board_['height']): for c in range(self.board_['width']): src_pts.append(np.array([r * self.board_['square_size'] , c * self.board_['square_size']] , dtype='float32') + self.board_['offset']) # OpenCV labels corners left-to-right, top-to-bottom # so we reverse the groud points src_pts.reverse() # Compute homography from image to ground self.H, mask = cv2.findHomography(corners2.reshape(len(corners2), 2), np.array(src_pts), cv2.RANSAC) extrinsics_filename = get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml" self.write_homography(extrinsics_filename) logger.info("Wrote ground projection to {}".format(extrinsics_filename))
def do_all_checks(): username = getpass.getuser() hostname = socket.gethostname() filename = 'what_the_duck-%s-%s.html' % (hostname, username) WTD = colored('what_the_duck', 'cyan', attrs=['bold']) entries = get_checks() # logger.info("%s checks many things about the Duckiebot configuration" % WTD) logger.info('%s will run %s tests.\n' % (WTD, len(entries))) results = run_checks(entries) o = "" o += display_results(results) + '\n\n' o += display_summary(results) + '\n\n' print(escaped_from_html(o)) write_data_to_file(o, filename) print('\nNow send the file "%s" to the TA/instructors.' % filename) print('\nYou can also upload it using the following command: ') print('\n scp %s [email protected]:%s ' % (filename, filename)) stats = Statistics(results) if stats.nfailures == 0: sys.exit(0) else: sys.exit(stats.nfailures)
def write_calibration(self): '''Load kinematic calibration file''' home = expanduser("~") filename = (home + "/duckietown_sysid/kinematics/" + self.robot_name + ".yaml") if not os.path.isfile(filename): logger.warn("no kinematic calibration parameters for {}, taking some from default".format(self.robot_name)) filename = (home+"/duckietown_sysid/kinematics/default.yaml") if not os.path.isfile(filename): logger.error("can't find default either, something's wrong, is the duckiefleet root correctly set?") else: data = yaml_load_file(filename) else: rospy.loginfo("Loading some kinematic calibration parameters of " + self.robot_name) data = yaml_load_file(filename) logger.info("Loaded homography for {}".format(os.path.basename(filename))) # Load some of the parameters that will not be changed param_k = data['k'] param_limit = data['limit'] param_radius = data['radius'] baseline = data['baseline'] # simply to increase readability gain = self.fit_['gain'] trim = self.fit_['trim'] # Write to yaml #datasave = { # This is similar to the inverse_kinematics_node, but it did not work... # "calibration_time": time.strftime("%Y-%m-%d-%H-%M-%S"), # "gain": gain, # "trim": trim, # "baseline": baseline, # "radius": param_radius, # "k": param_k, # "limit": param_limit, #} datasave = \ "calibration_time: {}".format(time.strftime("%Y-%m-%d-%H-%M-%S")) + \ "\ngain: {}".format(gain) + \ "\ntrim: {}".format(trim) + \ "\nbaseline: {}".format(baseline) + \ "\nradius: {}".format(param_radius) + \ "\nk: {}".format(param_k) + \ "\nlimit: {}".format(param_limit) print("\nThe Estimated Kinematic Calibration is:") print("gain = {}".format(gain)) print("trim = {}".format(trim)) # Write to yaml file filename = ( home + "/duckietown_sysid/kinematics/" + self.robot_name + ".yaml") with open(filename, 'w') as outfile: outfile.write(datasave) #outfile.write(yaml.dump(datasave, default_flow_style=False)) # This did not work and gave very weird results print("Saved Parameters to " + self.robot_name + ".yaml" ) print("\nPlease check the plots and judge if the parameters are reasonable.") print("Once done inspecting the plot, close them to terminate the program.")
def load_camera_info(self): '''Load camera intrinsics''' filename = (sys.path[0] + "/calibrations/camera_intrinsic/" + self.robot_name + ".yaml") if not os.path.isfile(filename): logger.warn( "no intrinsic calibration parameters for {}, trying default". format(self.robot_name)) filename = (sys.path[0] + "/calibrations/camera_intrinsic/default.yaml") if not os.path.isfile(filename): logger.error("can't find default either, something's wrong") calib_data = yaml_load_file(filename) # logger.info(yaml_dump(calib_data)) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape( (3, 3)) cam_info.D = np.array( calib_data['distortion_coefficients']['data']).reshape((1, 5)) cam_info.R = np.array( calib_data['rectification_matrix']['data']).reshape((3, 3)) cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape( (3, 4)) cam_info.distortion_model = calib_data['distortion_model'] logger.info( "Loaded camera calibration parameters for {} from {}".format( self.robot_name, os.path.basename(filename))) return cam_info
def download_url_to_file(url, filename): logger.info('Download from %s' % (url)) tmp = filename + '.download' cmd = ['wget', '-O', tmp, url] d8n_make_sure_dir_exists(tmp) res = system_cmd_result(cwd='.', cmd=cmd, display_stdout=False, display_stderr=False, raise_on_error=True, write_stdin='', capture_keyboard_interrupt=False, env=None) if not os.path.exists(tmp): msg = 'Downloaded file does not exist but wget did not give any error.' msg += '\n url: %s' % url msg += '\n downloaded to: %s' % tmp msg += '\n' + indent(str(res), ' | ') d = os.path.dirname(tmp) r = system_cmd_result(d, ['ls', '-l'], display_stdout=False, display_stderr=False, raise_on_error=True) msg += '\n Contents of the directory:' msg += '\n' + indent(str(r.stdout), ' | ') raise Exception(msg) os.rename(tmp, filename) logger.info('-> %s' % friendly_path(filename))
def upload(to_upload): filename = '/tmp/what_the_duck' S = shelve.open(filename) try: # logger.debug('New records: %s previous: %s' % (len(to_upload), len(S))) for u in to_upload: S[u['_id']] = u if not is_internet_connected(): msg = 'Internet is not connected: cannot upload results.' logger.warning(msg) else: remaining = [] for k in S: remaining.append(S[k]) collection = get_upload_collection() logger.info('Uploading %s test results' % len(remaining)) collection.insert_many(remaining) logger.info('done') for r in remaining: del S[r['_id']] finally: # logger.info('Remaining %s' % (len(S))) S.close()
def what_the_duck_stats(): if len(sys.argv) > 1: output = sys.argv[1] else: output = 'what_the_duck_stats.html' collection = get_upload_collection() res = list(get_valid_data(collection)) # for r in res: # del r['_id'] # data = yaml_dump(res) write_data_to_file(data, 'last_download.yaml') hostnames = defaultdict(lambda: 0) for r in res: hostnames[r['hostname']] += 1 s = 'Number of tests by hostname: ' for h, n in hostnames.items(): s += '\n- %s: %d tests' % (h, n) logger.info(s) html = create_summary(res) write_data_to_file(html, output)
def estimate_homography(self, cv_image): '''Estimate ground projection using instrinsic camera calibration parameters''' cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) cv_image_rectified = self.rectify(cv_image) logger.info("image rectified") ret, corners = cv2.findChessboardCorners( cv_image_rectified, (self.board_['width'], self.board_['height']), cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE) if ret == False: logger.error("No corners found in image") exit(1) if len(corners) != self.board_['width'] * self.board_['height']: logger.error("Not all corners found in image") exit(2) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01) corners2 = cv2.cornerSubPix(cv_image_rectified, corners, (11, 11), (-1, -1), criteria) cv2.drawChessboardCorners(cv_image_rectified, (7, 5), corners2, ret) cv2.imshow('corners', cv_image_rectified) cv2.waitKey(10000) cv2.destroyAllWindows() #TODO flip checks src_pts = [] for r in range(self.board_['height']): for c in range(self.board_['width']): src_pts.append( np.array([ r * self.board_['square_size'], c * self.board_['square_size'] ], dtype='float32') + self.board_['offset']) # OpenCV labels corners left-to-right, top-to-bottom # We're having a problem with our pattern since it's not rotation-invariant # only reverse order if first point is at bottom right corner if ((corners[0])[0][0] < (corners[self.board_['width'] * self.board_['height'] - 1])[0][0] and (corners[0])[0][0] < (corners[self.board_['width'] * self.board_['height'] - 1])[0][1]): rospy.loginfo("Reversing order of points.") src_pts.reverse() # Compute homography from image to ground self.H, mask = cv2.findHomography(corners2.reshape(len(corners2), 2), np.array(src_pts), cv2.RANSAC) extrinsics_filename = get_duckiefleet_root( ) + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml" self.write_homography(extrinsics_filename) logger.info( "Wrote ground projection to {}".format(extrinsics_filename)) # Check if specific point in matrix is larger than zero (this would definitly mean we're having a corrupted rotation matrix) if (self.H[1][2] > 0): rospy.logerr("WARNING: Homography could be corrupt!")
def get_upload_collection(): s = get_connection_string() logger.info('Opening connection to MongoDB...') client = pymongo.MongoClient(s) db = client[mongo_db] collection = db[mongo_collection] return collection
def job_analyze(log, analyzer): easy_algo_db = get_easy_algo_db() analyzer_instance = easy_algo_db.create_instance('analyzer', analyzer) in_bag = rosbag.Bag(log) results = OrderedDict() logger.info('Running %s on %s' % (analyzer, log)) analyzer_instance.analyze_log(in_bag, results) in_bag.close() return results
def download_if_not_exist(url, filename): if not os.path.exists(filename): logger.info('Path does not exist: %s' % filename) download_url_to_file(url, filename) if not os.path.exists(filename): msg = 'I expected download_url_to_file() to raise an error if failed.' msg += '\n url: %s' % url msg += '\n filename: %s' % filename raise AssertionError(msg) return filename
def load_map(map_filename): if not isfile(map_filename): print('Map does not exist') exit(1) map_name = splitext(basename(map_filename))[0] with open(map_filename) as f: contents = f.read() data = yaml_load(contents) logger.info('Loaded {} map data'.format(map_name)) return data
def calculate_transform(image): """ Returns tuple (bool, float, parameters) success, health, parameters parameters['scale'] parameters['shift'] """ centers4 = CENTERS2 trained4, counter4, score4 = runKMeans(image, num_colors=4, init=centers4) trained4 = trained4[[0, 2, 3], :] counter4 = [counter4[0], counter4[2], counter4[3]] centers4 = centers4[[0, 2, 3], :] centers3 = CENTERS trained3, counter3, score3 = runKMeans(image, num_colors=3, init=centers3) decision34 = ((score3 + 3e7) > score4) > score4 if (decision34): logger.info("picked 3 colors") trained = trained3 counter = counter3 centers = centers3 else: logger.info("picked 4 colors") trained = trained4 counter = counter4 centers = centers4 # milansc: for debug reasons print('Score for 3 colors: ' + str(score3)) print('Score for 4 colors: ' + str(score4)) print('...in calculate_transform: found centers:') print(centers) print('...in calculate_transform: found counter:') print(counter) print('...in calculate_transform: found trained centers:') print(trained) mapping = identifyColors(trained, centers) r, g, b, cost = getparameters2(mapping, trained, counter, centers) if r[0][0] == 0.0: # XXX: not sure what this is supposed to be return False, 0.0, None scale = np.array([r[0][0][0], g[0][0][0], b[0][0][0]]) shift = np.array([r[1][0], g[1][0], b[1][0]]) eps = np.finfo('double').eps health = 1.0 / (cost + eps) parameters = dict(scale=scale, shift=shift) return True, float(health), parameters
def read_scuderia(scuderia): """ Returns the contents of scuderia as a dict, with validated fields.""" if not os.path.exists(scuderia): msg = 'Scuderia file %s does not exist' % scuderia raise ScuderiaException(msg) import yaml yaml_string = open(scuderia).read() try: values = yaml.load(yaml_string) except yaml.YAMLError as e: msg = 'Yaml file is invalid:\n---\n%s' % e raise ScuderiaException(msg) if not isinstance(values, dict): msg = 'Invalid content: %s' % values raise ScuderiaException(msg) n = len(values) names = ", ".join(sorted(values)) logger.info('I found %d Duckiebots in the scuderia: %s.' % (n, names)) results = {} for k, value in values.items(): try: check_good_name(k) if not isinstance(value, dict): msg = 'Entry should be a dict, found %s' % k raise_desc(ScuderiaException, msg) fields = ['owner_duckietown_id', 'username', 'owner_duckietown_id'] for f in fields: if not f in value: msg = 'Could not find field "%s".' % f raise ScuderiaException(msg) owner_duckietown_id = value['owner_duckietown_id'] username = value['username'] robot_name = k results[robot_name] = ScuderiaEntry(robot_name=robot_name, username=username, owner_duckietown_id=owner_duckietown_id) except ScuderiaException as e: msg = 'Invalid entry "%s" in scuderia.' % k msg += '\n\n' + indent(yaml.dump(value).strip(), '> ') + '\n ' raise_wrapped(ScuderiaException, e, msg, compact=True) return results
def load_homography(veh): path = '/calibrations/camera_' filename = get_duckiefleet_root() + path + 'extrinsic/' + veh + '.yaml' if not isfile(filename): print('Extrinsic calibration for {} does not exist.'.format(veh)) exit(2) with open(filename) as f: contents = f.read() data = yaml_load(contents) logger.info('Loaded homography for {}'.format(veh)) return np.array(data['homography']).reshape(3,3)
def write_to_file_if_changed(filename, contents): if os.path.exists(filename): existing = open(filename).read() need_write = existing != contents else: need_write = True if need_write: with open(filename, 'w') as f: f.write(contents) logger.info('Written to %s' % filename) else: logger.info('File already up to date %s' % filename)
def get_scuderia_contents(): ''' Returns the contents of the scuderia file from $(DUCKIEFLEET_ROOT)/scuderia.yaml ''' fn_scuderia = get_scuderia_path() try: scuderia_contents = read_scuderia(fn_scuderia) except ScuderiaException as e: msg = 'Invalid scuderia file %r.' % fn_scuderia raise_wrapped(ScuderiaException, e, msg, compact=True) logger.info('Read %d entries from scuderia %s' % (len(scuderia_contents), fn_scuderia)) return scuderia_contents
def write_to_dir(data, d): if isinstance(data, dict): if not os.path.exists(d): os.makedirs(d) for k, v in data.items(): write_to_dir(v, os.path.join(d, k)) elif isinstance(data, str): with open(d, 'w') as f: f.write(data) logger.info('Wrote %s' % d) else: msg = 'Invalid type.' raise_desc(ValueError, msg, data=data, d=d)
def d8n_make_video_from_bag(bag_filename, topic, out): """ Creates a video out of the topic in the bag. topic: the topic name (any image-like topic) out: an .mp4 file. Note that needs a bunch more dependencies to be installed. Until we fix the dependencies: sudo pip install SystemCmd==1.2 ros_node_utils==1.0 ConfTools==1.8 QuickApp==1.2.2 sudo apt-get install -y mplayer mencoder sudo add-apt-repository ppa:mc3man/trusty-media sudo apt-get update sudo apt-get install -y ffmpeg #gstreamer0.10-ffmpeg """ try: import procgraph_ros # @UnusedImport from procgraph import pg except ImportError: raise # pg -m procgraph_ros bag2mp4 --bag $bag --topic $topic --out $out model = 'bag2mp4_fixfps' # model = 'bag2mp4' tmpdir = create_tmpdir() out_tmp = os.path.join(tmpdir, os.path.basename(out)) logger.debug('Writing temp file to %s' % out_tmp) logger.debug('(You can use mplayer to follow along.)') pg(model, config=dict(bag=bag_filename, topic=topic, out=out_tmp)) md = out_tmp + '.metadata.yaml' if os.path.exists(md): os.unlink(md) dn = os.path.dirname(out) if not os.path.exists(dn): os.makedirs(dn) shutil.copyfile(out_tmp, out) logger.info('Created: %s' % out) info = out_tmp + '.info.yaml' if os.path.exists(info): os.unlink(info)
def d8n_write_to_bag_context(out_bag_filename): """ with d8n_write_to_bag_context(filename) as bag: bag.write(topic_name, msg) """ import rosbag # @UnresolvedImport d8n_make_sure_dir_exists(out_bag_filename) out_bag = rosbag.Bag(out_bag_filename + '.tmp', 'w') yield out_bag out_bag.close() os.rename(out_bag_filename + '.tmp', out_bag_filename) logger.info('Written bag to %s' % out_bag_filename)
def load_homography(self): '''Load homography (extrinsic parameters)''' filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml") if not os.path.isfile(filename): logger.warn("no extrinsic calibration parameters for {}, trying default".format(self.robot_name)) filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/default.yaml") if not os.path.isfile(filename): logger.error("can't find default either, something's wrong") else: data = yaml_load_file(filename) else: data = yaml_load_file(filename) logger.info("Loaded homography for {}".format(os.path.basename(filename))) return np.array(data['homography']).reshape((3,3))
def upload_results(results): to_upload = [] d0 = get_local_keys() for result in results: d1 = json_from_result(result) d1.update(d0) to_upload.append(d1) collection = get_upload_collection() logger.info('Inserting %s tests' % len(to_upload)) collection.insert_many(to_upload) logger.info('done')
def dropbox_links_main(query): logger.info('NOTE: Run this inside ~/Dropbox/duckietown-data. ') logger.info( 'NOTE: There are some hard-coded paths to modify in dropbox_links.py') output = get_urls_path() if os.path.exists(output): urls = yaml.safe_load(open(output).read()) for k, v in list(urls.items()): if not v.startswith('http'): del urls[k] else: urls = {} command = '/home/andrea/bin/dropbox' base = '/mnt/dorothy-duckietown-data/' db = get_easy_logs_db() logs = db.query(query) logger.info('Found %d logs.' % len(logs)) for logname, log in logs.items(): if logname in urls: logger.info('Already have %s' % logname) continue filename = log.filename only = filename.replace(base, '') cmd = [command, 'sharelink', only] res = system_cmd_result(cwd='.', cmd=cmd, display_stdout=False, display_stderr=True, raise_on_error=True, write_stdin='', capture_keyboard_interrupt=False, env=None) link = res.stdout.strip() if 'responding' in link: logger.debug('Dropbox is not responding, I will stop here.') break logger.info('link : %s' % link) urls[logname] = link yaml.default_flow_style = True # yaml.indent(mapping=2, sequence=4, offset=2) with open(output, 'w') as f: yaml.dump(urls, f)
def load_board_info(self, filename=''): '''Load calibration checkerboard info''' if not os.path.isfile(filename): filename = get_ros_package_path('ground_projection') + '/config/ground_projection_node/default.yaml' target_data = yaml_load_file(filename) target_info = { 'width': target_data['board_w'], 'height': target_data['board_h'], 'square_size': target_data['square_size'], 'x_offset': target_data['x_offset'], 'y_offset': target_data['y_offset'], 'offset': np.array([target_data['x_offset'], -target_data['y_offset']]), 'size': (target_data['board_w'], target_data['board_h']), } logger.info("Loaded checkerboard parameters") return target_info
def get_easy_logs_db_cloud(): cloud_file = require_resource('cloud.yaml') # cloud_file = os.path.join(get_ros_package_path('easy_logs'), 'cloud.yaml') # if not os.path.exists(cloud_file): # url = "https://www.dropbox.com/s/vdl1ej8fihggide/duckietown-cloud.yaml?dl=1" # download_url_to_file(url, cloud_file) logger.info('Loading cloud DB %s' % friendly_path(cloud_file)) logs = yaml_load_file(cloud_file) logs = OrderedDict(logs) logger.info('Loaded cloud DB with %d entries.' % len(logs)) return EasyLogsDB(logs)
def load_camera_intrinsics(veh): path = '/calibrations/camera_' filename = get_duckiefleet_root() + path + 'intrinsic/' + veh + '.yaml' if not isfile(filename): print('Intrinsic calibration for {} does not exist.'.format(veh)) exit(3) with open(filename) as f: contents = f.read() data = yaml_load(contents) intrinsics = {} intrinsics['K'] = np.array(data['camera_matrix']['data']).reshape(3, 3) intrinsics['D'] = np.array(data['distortion_coefficients']['data']).reshape(1, 5) intrinsics['R'] = np.array(data['rectification_matrix']['data']).reshape(3, 3) intrinsics['P'] = np.array(data['projection_matrix']['data']).reshape((3,4)) intrinsics['distortion_model'] = data['distortion_model'] logger.info('Loaded camera intrinsics for {}'.format(veh)) return intrinsics
def estimate_homography(self,cv_image, board_size, offset): cv_image_rectified = self.rectify(cv_image) logger.info("image rectified") corners = cv2.findChessboardCorners(cv_image, board_size) if corners is None: logger.error("No corners found in image") criteria = (cv2.CV_TERMCRIT_EPS + cv2.CV_TERMCRIT_ITER,30,0.1) cv2.cornerSubPix(cv_image,corners,cv2.Size(11,11),cv2.Size(-1,-1),criteria) #TODO flip checks for r in range(board_h): for c in range(board_w): src_pts[r,c] = np.float32([r*square_size,c*square_size]) + offset self.H, mask = cv2.findHomography(src_pts, corners, method=cv2.CV_RANSAC) write_homography(self.extrinsic_filename)
def call_summary(): db = get_easy_algo_db() s = format_db(db) logger.info(s) errors = [] for f in db.family_name2config.values(): if not f.valid: errors.append('Family %s: %s' % (f.family_name, f.error_if_invalid)) for i in f.instances.values(): if not i.valid: errors.append('Family %s / instance %r: %s' % (f, i.instance_name, i.error_if_invalid)) if errors: msg = '\n' + '\n'.join(f.instances) raise Exception(msg)
def get_valid_data(collection): res = list(collection.find()) out = [] for r in res: if 'what_the_duck_version' in r: v = r['what_the_duck_version'] if v in ['1.1', '1.2']: del r['_id'] if v in ['1.1']: r['type'] = '?' out.append(r) logger.info('Found %d database entries; %d are compatible.' % (len(res), len(out))) return out
def search_all_configuration_files(): sources = [] # We look in $DUCKIETOWN_ROOT/catkin_ws/src sources.append(get_catkin_ws_src()) # then we look in $DUCKIETOWN_FLEET sources.append(get_duckiefleet_root()) configs = [] pattern = '*' + SUFFIX logger.info('Looking for %s files.' % pattern) for s in sources: fs = locate_files(s, pattern) logger.info('%4d files in %s' % (len(fs), s)) configs.extend(fs) # logger.debug('I found:\n' + "\n".join(configs)) return configs