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