def __init__(self, start_pose: np.matrix, sensor_angles, config: de2bot.DE2Config, beacons): state = de2bot.State(start_pose) self.angles = sensor_angles self.prev_controls = np.asmatrix([0, 0.]).T self.next_sense = 0 self.last_sense = None self.localization = Localizer(state, config, sensor_angles, beacons)
def unknown_data(loc: localization.Localizer, modes: List[str]) -> Dict[str, str]: server_data = {} if 'Server name' in modes: server_data['server_name'] = loc.text("Unknown server name") if 'Player count' in modes: server_data['player_count'] = loc.text("Players: {0}/{1}").format( "?", "?") if 'Kills' in modes: server_data['kills'] = loc.text("Kills: {0}").format("?") return server_data
class Controller: def __init__(self, start_pose: np.matrix, sensor_angles, config: de2bot.DE2Config, beacons): state = de2bot.State(start_pose) self.angles = sensor_angles self.prev_controls = np.asmatrix([0, 0.]).T self.next_sense = 0 self.last_sense = None self.localization = Localizer(state, config, sensor_angles, beacons) def estimated_position(self): return self.localization.robot.state.pose def update(self, encoders: np.matrix, meas_dist: float, can_sense: bool, delta_time: float): """ Update logic for the controller. Called at 100Hz. Parameters: encoders: wheel velocities meas_dist: previous sonar measurement distance, or None can_sense: are we able to sense from sonar delta_time: time since last update Return: (controls, sensor number) """ controls = np.asmatrix([ [0.2], [0.3], ]) # Kalman Filter predict step self.localization.predict(encoders, delta_time) # R = np.asmatrix([[1, 0],[0, 1]]) R = None if meas_dist is not None: # Update EKF # pass R = self.localization.update_beacon(meas_dist, self.last_sense) self.prev_controls = controls sensor = None if can_sense: sensor = self.next_sense self.last_sense = sensor self.next_sense = (self.next_sense + 1) % len(self.angles) return controls, sensor, R
def load_localizer(params='/app/params'): src = os.path.join(params, 'params_localization') with open(os.path.join(src, 'post_config.json')) as file: data = json.load(file) localizer = Localizer(os.path.join(src, 'config.py'), os.path.join(src, 'weights.pth'), data['confidence_th']) return localizer
def __init__(self, working_dir, src, buggy, oracle, tests, golden, asserts, lines, build, configure, config): self.working_dir = working_dir self.config = config self.repair_test_suite = tests[:] self.validation_test_suite = tests[:] extracted = join(working_dir, 'extracted') os.mkdir(extracted) angelic_forest_file = join(working_dir, 'last-angelic-forest.json') tester = Tester(config, oracle, abspath(working_dir)) self.run_test = tester self.get_suspicious_groups = Localizer(config, lines) self.reduce = Reducer(config) if self.config['use_semfix_syn']: self.synthesize_fix = Semfix_Synthesizer(working_dir, config, extracted, angelic_forest_file) self.infer_spec = Semfix_Inferrer(working_dir, config, tester) else: self.synthesize_fix = Synthesizer(config, extracted, angelic_forest_file) self.infer_spec = Inferrer(config, tester, Load(working_dir)) self.instrument_for_localization = RepairableTransformer(config) self.instrument_for_inference = SuspiciousTransformer(config, extracted) self.apply_patch = FixInjector(config) validation_dir = join(working_dir, "validation") shutil.copytree(src, validation_dir, symlinks=True) self.validation_src = Validation(config, validation_dir, buggy, build, configure) self.validation_src.configure() compilation_db = self.validation_src.export_compilation_db() self.validation_src.import_compilation_db(compilation_db) self.validation_src.initialize() frontend_dir = join(working_dir, "frontend") shutil.copytree(src, frontend_dir, symlinks=True) self.frontend_src = Frontend(config, frontend_dir, buggy, build, configure) self.frontend_src.import_compilation_db(compilation_db) self.frontend_src.initialize() backend_dir = join(working_dir, "backend") shutil.copytree(src, backend_dir, symlinks=True) self.backend_src = Backend(config, backend_dir, buggy, build, configure) self.backend_src.import_compilation_db(compilation_db) self.backend_src.initialize() if golden is not None: golden_dir = join(working_dir, "golden") shutil.copytree(golden, golden_dir, symlinks=True) self.golden_src = Frontend(config, golden_dir, buggy, build, configure) self.golden_src.import_compilation_db(compilation_db) self.golden_src.initialize() else: self.golden_src = None self.dump = Dump(working_dir, asserts) self.trace = Trace(working_dir)
def __init__(self): self.target_identifier = TargetIdentifier() self.localizer = Localizer() self.bridge = CvBridge() self.sub_cam = rospy.Subscriber("/ardrone/bottom/image_raw", Image, self.identify_targets) self.sub_pos = rospy.Subscriber("/vicon/ARDroneCarre/ARDroneCarre", TransformStamped, self.store_pos) self.ob_locations = [] self.radius_list = [] self.lm_locations = [] self.crop_list = [] #self.yaw_list = [] self.epsilon = Duration(5) self.frames = 0 self.obstacle_frames = 0 self.landmark_frames = 0 self.not_pixels = 0 self.time_filtered = 0 return
class Processor(): def __init__(self): self.target_identifier = TargetIdentifier() self.localizer = Localizer() self.bridge = CvBridge() self.sub_cam = rospy.Subscriber("/ardrone/bottom/image_raw", Image, self.identify_targets) self.sub_pos = rospy.Subscriber("/vicon/ARDroneCarre/ARDroneCarre", TransformStamped, self.store_pos) self.ob_locations = [] self.radius_list = [] self.lm_locations = [] self.crop_list = [] #self.yaw_list = [] self.epsilon = Duration(5) self.frames = 0 self.obstacle_frames = 0 self.landmark_frames = 0 self.not_pixels = 0 self.time_filtered = 0 return def identify_targets(self, msg): self.frames +=1 # convert bag image to cv2 image img = self.bridge.imgmsg_to_cv2(msg, "bgr8") # get pixel location for obstacles and landmarks ob_pixels, radius_pixel = self.target_identifier.identify_obstacle(img) lm_pixels, crop_img, yaw = self.target_identifier.identify_landmark(img, self.pos) # if no obstacles or landmark are found if not ob_pixels and not lm_pixels: self.not_pixels+=1 return else: # if obstacles are found if ob_pixels: self.obstacle_frames+=1 # convert pixel location of centroids to world frame new_locations = self.localizer.localize(ob_pixels, self.pos) self.ob_locations += new_locations # convert pixel location of edge point to world frame point = self.localizer.localize(radius_pixel, self.pos) # calculate radius radius = [] for i in range(len(point)): radius.append(np.sqrt((new_locations[i][0] - point[i][0]) ** 2 + (new_locations[i][1] - point[i][1]) ** 2)) self.radius_list += radius # # keep track of obstacle locations # f = open('/home/demi/aer1217/labs/src/processor/ob_location.txt', 'a') # for i in range(len(new_locations)): # f.writelines('\n' + np.array2string(new_locations[i][0])+' '+ np.array2string(new_locations[i][1])+' '+ str(radius[i])) # f.close() # if landmarks are found if lm_pixels: self.landmark_frames += 1 # convert pixel location of centroids to world frame new_locations = self.localizer.localize(lm_pixels, self.pos) self.lm_locations += new_locations # # keep track of landmark locations # f = open('/home/demi/aer1217/labs/src/processor/lm_location.txt', 'a') # for i in range(len(new_locations)): # f.writelines('\n' + np.array2string(new_locations[i][0]) + ' ' + np.array2string(new_locations[i][1])) # f.close() # save crop images of landmark for further process self.crop_list.append(crop_img) # save current yaw angle of the drone for further process f = open('/home/demi/aer1217/labs/src/processor/lm_pose.txt', 'a') for i in range(len(yaw)): f.writelines('\n' + str(yaw[i])) f.close() return def store_pos(self, msg): self.pos = msg return