From dedc6da36302939e4acaf569519d124c23593210 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 15 May 2025 13:49:46 +0200 Subject: [PATCH 01/18] Use input container for lh geo estimator --- .../lighthouse_geo_bs_estimation_wizard.py | 48 +++++++++---------- 1 file changed, 22 insertions(+), 26 deletions(-) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index 63ef456d..edefcd21 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -41,7 +41,10 @@ from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler -from cflib.localization.lighthouse_types import Pose, LhDeck4SensorPositions, LhMeasurement, LhCfPoseSample +from cflib.localization.lighthouse_types import Pose, LhDeck4SensorPositions, LhMeasurement +from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample +from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer + from PyQt6 import QtCore, QtWidgets, QtGui import time @@ -173,7 +176,6 @@ def _timeout_cb(self): self.timeout_timer.stop() def _ready_cb(self, averages): - print(self.show_add_measurements) recorded_angles = averages angles_calibrated = {} for bs_id, data in recorded_angles.items(): @@ -327,18 +329,15 @@ class EstimateGeometryThread(QtCore.QObject): finished = QtCore.pyqtSignal() failed = QtCore.pyqtSignal() - def __init__(self, origin, x_axis, xy_plane, samples): + def __init__(self, container: LhGeoInputContainer): super(EstimateGeometryThread, self).__init__() - self.origin = origin - self.x_axis = x_axis - self.xy_plane = xy_plane - self.samples = samples + self.container = container self.bs_poses = {} def run(self): try: - self.bs_poses = self._estimate_geometry(self.origin, self.x_axis, self.xy_plane, self.samples) + self.bs_poses = self._estimate_geometry(self.container) self.finished.emit() except Exception as ex: print(ex) @@ -347,27 +346,21 @@ def run(self): def get_poses(self): return self.bs_poses - def _estimate_geometry(self, origin: LhCfPoseSample, - x_axis: list[LhCfPoseSample], - xy_plane: list[LhCfPoseSample], - samples: list[LhCfPoseSample]) -> dict[int, Pose]: + def _estimate_geometry(self, container: LhGeoInputContainer) -> dict[int, Pose]: """Estimate the geometry of the system based on samples recorded by a Crazyflie""" - matched_samples = [origin] + x_axis + xy_plane + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2) - initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, - LhDeck4SensorPositions.positions) + matched_samples = container.get_matched_samples() + initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples) - solution = LighthouseGeometrySolver.solve(initial_guess, - cleaned_matched_samples, - LhDeck4SensorPositions.positions) + solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions) if not solution.success: raise Exception("No lighthouse base station geometry solution could be found!") start_x_axis = 1 - start_xy_plane = 1 + len(x_axis) + start_xy_plane = 1 + len(container.x_axis) origin_pos = solution.cf_poses[0].translation - x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(x_axis)] + x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(container.x_axis)] x_axis_pos = list(map(lambda x: x.translation, x_axis_poses)) - xy_plane_poses = solution.cf_poses[start_xy_plane:start_xy_plane + len(xy_plane)] + xy_plane_poses = solution.cf_poses[start_xy_plane:start_xy_plane + len(container.xy_plane)] xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses)) # Align the solution @@ -407,12 +400,15 @@ def __init__(self, cf: Crazyflie, origin_page: RecordOriginSamplePage, xaxis_pag def _action_btn_clicked(self): self.start_action_button.setDisabled(True) self.status_text.setText(self.str_pad('Estimating geometry...')) - origin = self.origin_page.get_sample() - x_axis = [self.xaxis_page.get_sample()] - xy_plane = self.xyplane_page.get_samples() - samples = self.xyzspace_page.get_samples() + + container = LhGeoInputContainer(LhDeck4SensorPositions.positions) + container.set_origin_sample(self.origin_page.get_sample()) + container.set_x_axis_sample(self.xaxis_page.get_sample()) + container.set_xy_plane_samples(self.xyplane_page.get_samples()) + container.set_xyz_space_samples(self.xyzspace_page.get_samples()) + self.thread_estimator = QtCore.QThread() - self.worker = EstimateGeometryThread(origin, x_axis, xy_plane, samples) + self.worker = EstimateGeometryThread(container) self.worker.moveToThread(self.thread_estimator) self.thread_estimator.started.connect(self.worker.run) self.worker.finished.connect(self.thread_estimator.quit) From 03a0b19dd711b22d352d791e073f3ebec2c0bbfe Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 15 May 2025 14:12:40 +0200 Subject: [PATCH 02/18] Added debug support --- .../ui/wizards/lighthouse_geo_bs_estimation_wizard.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index edefcd21..f4e352f9 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -180,7 +180,7 @@ def _ready_cb(self, averages): angles_calibrated = {} for bs_id, data in recorded_angles.items(): angles_calibrated[bs_id] = data[1] - self.recorded_angle_result = LhCfPoseSample(angles_calibrated=angles_calibrated) + self.recorded_angle_result = LhCfPoseSample(angles_calibrated) self.visible_basestations = ', '.join(map(lambda x: str(x + 1), recorded_angles.keys())) amount_of_basestations = len(recorded_angles.keys()) @@ -276,7 +276,6 @@ def __init__(self, cf: Crazyflie, parent=None): self.record_time_total = DEFAULT_RECORD_TIME self.record_time_current = 0 self.reader = LighthouseSweepAngleReader(self.cf, self._ready_single_sample_cb) - self.bs_seen = set() def extra_layout_field(self): h_box = QtWidgets.QHBoxLayout() @@ -319,7 +318,6 @@ def _ready_single_sample_cb(self, bs_id: int, angles: LighthouseBsVectors): now = time.time() measurement = LhMeasurement(timestamp=now, base_station_id=bs_id, angles=angles) self.recorded_angles_result.append(measurement) - self.bs_seen.add(str(bs_id + 1)) def get_samples(self): return self.recorded_angles_result @@ -407,6 +405,12 @@ def _action_btn_clicked(self): container.set_xy_plane_samples(self.xyplane_page.get_samples()) container.set_xyz_space_samples(self.xyzspace_page.get_samples()) + # Enable to write to file. This can be used for debugging in examples/lighthouse/multi_bs_geometry_estimation.py + # found in the python lib + # import pickle + # with open('lh_geo_input_dump.pickle', 'wb') as handle: + # pickle.dump(container, handle, protocol=pickle.HIGHEST_PROTOCOL) + self.thread_estimator = QtCore.QThread() self.worker = EstimateGeometryThread(container) self.worker.moveToThread(self.thread_estimator) From bb6cea9ea10a4a9811b9ba6ea92aa508bfe92a62 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 15 May 2025 14:35:12 +0200 Subject: [PATCH 03/18] Added logging --- .../ui/wizards/lighthouse_geo_bs_estimation_wizard.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index f4e352f9..42b64b22 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -30,6 +30,8 @@ from __future__ import annotations import cfclient +import logging +import time from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry @@ -45,11 +47,11 @@ from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer - from PyQt6 import QtCore, QtWidgets, QtGui -import time +logger = logging.getLogger(__name__) + REFERENCE_DIST = 1.0 ITERATION_MAX_NR = 2 DEFAULT_RECORD_TIME = 20 @@ -335,10 +337,12 @@ def __init__(self, container: LhGeoInputContainer): def run(self): try: + logger.debug("Start estimation") self.bs_poses = self._estimate_geometry(self.container) + logger.debug("Estimation done") self.finished.emit() except Exception as ex: - print(ex) + logger.error("Estimation exception: " + str(ex)) self.failed.emit() def get_poses(self): From ecd86e84a19e345ef2a0fe615361e2da44fe14c5 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 16 May 2025 10:44:50 +0200 Subject: [PATCH 04/18] LH geo estimation scaling in cflib updated --- .../lighthouse_geo_bs_estimation_wizard.py | 52 +++++++++---------- 1 file changed, 24 insertions(+), 28 deletions(-) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index 42b64b22..16aab1eb 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -39,19 +39,16 @@ from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator -from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher -from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver -from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler from cflib.localization.lighthouse_types import Pose, LhDeck4SensorPositions, LhMeasurement from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample -from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer +from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer, LhGeoEstimationManager from PyQt6 import QtCore, QtWidgets, QtGui - logger = logging.getLogger(__name__) + REFERENCE_DIST = 1.0 ITERATION_MAX_NR = 2 DEFAULT_RECORD_TIME = 20 @@ -73,6 +70,8 @@ def __init__(self, cf, ready_cb, parent=None, *args): self.button(QtWidgets.QWizard.WizardButton.FinishButton).clicked.connect(self._finish_button_clicked_callback) + logger.info("Wizard started") + def _finish_button_clicked_callback(self): self.ready_cb(self.get_geometry_page.get_geometry()) @@ -337,9 +336,9 @@ def __init__(self, container: LhGeoInputContainer): def run(self): try: - logger.debug("Start estimation") + logger.info("Start estimation") self.bs_poses = self._estimate_geometry(self.container) - logger.debug("Estimation done") + logger.info("Estimation done") self.finished.emit() except Exception as ex: logger.error("Estimation exception: " + str(ex)) @@ -350,34 +349,28 @@ def get_poses(self): def _estimate_geometry(self, container: LhGeoInputContainer) -> dict[int, Pose]: """Estimate the geometry of the system based on samples recorded by a Crazyflie""" + matched_samples = container.get_matched_samples() + logger.info("start initial guess") initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples) + logger.info("initial guess done") + + scaled_initial_guess = LhGeoEstimationManager.align_and_scale_solution(container, initial_guess, REFERENCE_DIST) + for bs_id, pose in sorted(scaled_initial_guess.bs_poses.items()): + pos = pose.translation + logger.info(f' {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})') + logger.info(f"Len cleaned samples: {len(cleaned_matched_samples)}") + + logger.info("Start solver") solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions) + logger.info("Solver done") + logger.info(f"Success: {solution.success}") if not solution.success: raise Exception("No lighthouse base station geometry solution could be found!") - start_x_axis = 1 - start_xy_plane = 1 + len(container.x_axis) - origin_pos = solution.cf_poses[0].translation - x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(container.x_axis)] - x_axis_pos = list(map(lambda x: x.translation, x_axis_poses)) - xy_plane_poses = solution.cf_poses[start_xy_plane:start_xy_plane + len(container.xy_plane)] - xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses)) - - # Align the solution - bs_aligned_poses, transformation = LighthouseSystemAligner.align( - origin_pos, x_axis_pos, xy_plane_pos, solution.bs_poses) - - cf_aligned_poses = list(map(transformation.rotate_translate_pose, solution.cf_poses)) - - # Scale the solution - bs_scaled_poses, cf_scaled_poses, scale = LighthouseSystemScaler.scale_fixed_point(bs_aligned_poses, - cf_aligned_poses, - [REFERENCE_DIST, 0, 0], - cf_aligned_poses[1]) - - return bs_scaled_poses + scaled_solution = LhGeoEstimationManager.align_and_scale_solution(container, solution.poses, REFERENCE_DIST) + return scaled_solution.bs_poses class EstimateBSGeometryPage(LighthouseBasestationGeometryWizardBasePage): @@ -414,6 +407,9 @@ def _action_btn_clicked(self): # import pickle # with open('lh_geo_input_dump.pickle', 'wb') as handle: # pickle.dump(container, handle, protocol=pickle.HIGHEST_PROTOCOL) + # with open('lh_geo_input_dump.pickle', 'rb') as handle: + # container = pickle.load(handle) + self.thread_estimator = QtCore.QThread() self.worker = EstimateGeometryThread(container) From 16a89f9636cefbd2e8baebe3d806f14a37bd2718 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 12 Jun 2025 15:35:25 +0200 Subject: [PATCH 05/18] Basic continuous lh geo estimation --- .../dialogs/lighthouse_bs_geometry_dialog.py | 2 +- src/cfclient/ui/tabs/lighthouse_tab.py | 6 +- .../lighthouse_geo_bs_estimation_wizard.py | 233 +++++------------- 3 files changed, 64 insertions(+), 177 deletions(-) diff --git a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py index f8658bdc..bc6c84a6 100644 --- a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py +++ b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py @@ -156,7 +156,7 @@ def __init__(self, lighthouse_tab, *args): self._lighthouse_tab._helper.cf, self._sweep_angles_received_and_averaged_signal.emit) self._base_station_geometry_wizard = LighthouseBasestationGeometryWizard( - self._lighthouse_tab._helper.cf, self._base_station_geometery_received_signal.emit) + self._lighthouse_tab, self._base_station_geometery_received_signal.emit) self._lh_geos = None self._newly_estimated_geometry = {} diff --git a/src/cfclient/ui/tabs/lighthouse_tab.py b/src/cfclient/ui/tabs/lighthouse_tab.py index 8d296994..4407dc4a 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.py +++ b/src/cfclient/ui/tabs/lighthouse_tab.py @@ -46,6 +46,8 @@ from cflib.localization import LighthouseConfigWriter from cflib.localization import LighthouseConfigFileManager +from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry + from cfclient.ui.dialogs.lighthouse_bs_geometry_dialog import LighthouseBsGeometryDialog from cfclient.ui.dialogs.basestation_mode_dialog import LighthouseBsModeDialog from cfclient.ui.dialogs.lighthouse_system_type_dialog import LighthouseSystemTypeDialog @@ -358,7 +360,9 @@ def __init__(self, helper): self._is_connected = False self._update_ui() - def write_and_store_geometry(self, geometries): + def write_and_store_geometry(self, geometries: dict[int, LighthouseBsGeometry]): + # TODO krri Hanlde repeated quick writes. This is called from the geo wizard and write_and_store_config() will + # throw if there is an ongoing write if self._lh_config_writer: self._lh_config_writer.write_and_store_config(self._new_system_config_written_to_cf_signal.emit, geos=geometries) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index 16aab1eb..658e2b7e 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -38,12 +38,11 @@ from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors -from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator -from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver -from cflib.localization.lighthouse_types import Pose, LhDeck4SensorPositions, LhMeasurement +from cflib.localization.lighthouse_types import LhDeck4SensorPositions, LhMeasurement, LhBsCfPoses from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer, LhGeoEstimationManager + from PyQt6 import QtCore, QtWidgets, QtGui logger = logging.getLogger(__name__) @@ -61,20 +60,19 @@ class LighthouseBasestationGeometryWizard(QtWidgets.QWizard): - def __init__(self, cf, ready_cb, parent=None, *args): + def __init__(self, lighthouse_tab, ready_cb, parent=None, *args): super(LighthouseBasestationGeometryWizard, self).__init__(parent) - self.cf = cf + self.lighthouse_tab = lighthouse_tab + self.cf = lighthouse_tab._helper.cf + self.container = LhGeoInputContainer(LhDeck4SensorPositions.positions) + self.solver_thread = LhGeoEstimationManager.SolverThread(self.container, is_done_cb=self.solution_handler) + self.solver_thread.start() self.ready_cb = ready_cb self.wizard_opened_first_time = True self.reset() - self.button(QtWidgets.QWizard.WizardButton.FinishButton).clicked.connect(self._finish_button_clicked_callback) - logger.info("Wizard started") - def _finish_button_clicked_callback(self): - self.ready_cb(self.get_geometry_page.get_geometry()) - def reset(self): self.setWindowFlags(self.windowFlags() & ~QtCore.Qt.WindowType.WindowContextHelpButtonHint) self.setWindowFlags(self.windowFlags() & ~QtCore.Qt.WindowType.WindowCloseButtonHint) @@ -84,35 +82,44 @@ def reset(self): self.removePage(1) self.removePage(2) self.removePage(3) - self.removePage(4) - del self.get_origin_page, self.get_xaxis_page, self.get_xyplane_page - del self.get_xyzspace_page, self.get_geometry_page + del self._origin_page, self._xaxis_page, self._xyplane_page + del self._xyzspace_page else: self.wizard_opened_first_time = False - self.get_origin_page = RecordOriginSamplePage(self.cf, self) - self.get_xaxis_page = RecordXAxisSamplePage(self.cf, self) - self.get_xyplane_page = RecordXYPlaneSamplesPage(self.cf, self) - self.get_xyzspace_page = RecordXYZSpaceSamplesPage(self.cf, self) - self.get_geometry_page = EstimateBSGeometryPage( - self.cf, self.get_origin_page, self.get_xaxis_page, self.get_xyplane_page, self.get_xyzspace_page, self) + self._origin_page = RecordOriginSamplePage(self.cf, self.container, self) + self._xaxis_page = RecordXAxisSamplePage(self.cf, self.container, self) + self._xyplane_page = RecordXYPlaneSamplesPage(self.cf, self.container, self) + self._xyzspace_page = RecordXYZSpaceSamplesPage(self.cf, self.container, self) - self.addPage(self.get_origin_page) - self.addPage(self.get_xaxis_page) - self.addPage(self.get_xyplane_page) - self.addPage(self.get_xyzspace_page) - self.addPage(self.get_geometry_page) + self.addPage(self._origin_page) + self.addPage(self._xaxis_page) + self.addPage(self._xyplane_page) + self.addPage(self._xyzspace_page) self.setWindowTitle("Lighthouse Base Station Geometry Wizard") self.resize(WINDOW_STARTING_WIDTH, WINDOW_STARTING_HEIGHT) + def solution_handler(self, scaled_solution: LhBsCfPoses): + """Upload the geometry to the Crazyflie""" + geo_dict = {} + for bs_id, pose in scaled_solution.bs_poses.items(): + geo = LighthouseBsGeometry() + geo.origin = pose.translation.tolist() + geo.rotation_matrix = pose.rot_matrix.tolist() + geo.valid = True + geo_dict[bs_id] = geo + + self.lighthouse_tab.write_and_store_geometry(geo_dict) + class LighthouseBasestationGeometryWizardBasePage(QtWidgets.QWizardPage): - def __init__(self, cf: Crazyflie, show_add_measurements=False, parent=None): + def __init__(self, cf: Crazyflie, container: LhGeoInputContainer, show_add_measurements=False, parent=None): super(LighthouseBasestationGeometryWizardBasePage, self).__init__(parent) self.show_add_measurements = show_add_measurements self.cf = cf + self.container = container self.layout = QtWidgets.QVBoxLayout() self.explanation_picture = QtWidgets.QLabel() @@ -181,7 +188,6 @@ def _ready_cb(self, averages): angles_calibrated = {} for bs_id, data in recorded_angles.items(): angles_calibrated[bs_id] = data[1] - self.recorded_angle_result = LhCfPoseSample(angles_calibrated) self.visible_basestations = ', '.join(map(lambda x: str(x + 1), recorded_angles.keys())) amount_of_basestations = len(recorded_angles.keys()) @@ -198,6 +204,7 @@ def _ready_cb(self, averages): self.start_action_button.setText("Restart Measurement") self.start_action_button.setDisabled(False) else: + self.store_sample(angles_calibrated) self.too_few_bs = False status_text_string = f'Recording Done! Visible Base stations: {self.visible_basestations}\n' if self.show_add_measurements: @@ -214,6 +221,9 @@ def _ready_cb(self, averages): self.start_action_button.setText("Restart Measurement") self.start_action_button.setDisabled(False) + def store_sample(self, angles: LhCfPoseSample) -> None: + self.recorded_angle_result = angles + def get_sample(self): return self.recorded_angle_result @@ -228,18 +238,22 @@ def str_pad(self, string_msg): class RecordOriginSamplePage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, parent=None): - super(RecordOriginSamplePage, self).__init__(cf) + def __init__(self, cf: Crazyflie, container: LhGeoInputContainer, parent=None): + super(RecordOriginSamplePage, self).__init__(cf, container) self.explanation_text.setText( 'Step 1. Put the Crazyflie where you want the origin of your coordinate system.\n') pixmap = QtGui.QPixmap(cfclient.module_path + "/ui/wizards/bslh_1.png") pixmap = pixmap.scaledToWidth(PICTURE_WIDTH) self.explanation_picture.setPixmap(pixmap) + def store_sample(self, angles: LhCfPoseSample) -> None: + self.container.set_origin_sample(LhCfPoseSample(angles)) + super().store_sample(angles) + class RecordXAxisSamplePage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, parent=None): - super(RecordXAxisSamplePage, self).__init__(cf) + def __init__(self, cf: Crazyflie, container: LhGeoInputContainer, parent=None): + super(RecordXAxisSamplePage, self).__init__(cf, container) self.explanation_text.setText('Step 2. Put the Crazyflie on the positive X-axis,' + f' exactly {REFERENCE_DIST} meters from the origin.\n' + 'This will be used to define the X-axis as well as scaling of the system.') @@ -247,10 +261,14 @@ def __init__(self, cf: Crazyflie, parent=None): pixmap = pixmap.scaledToWidth(PICTURE_WIDTH) self.explanation_picture.setPixmap(pixmap) + def store_sample(self, angles: LhCfPoseSample) -> None: + self.container.set_x_axis_sample(LhCfPoseSample(angles)) + super().store_sample(angles) + class RecordXYPlaneSamplesPage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, parent=None): - super(RecordXYPlaneSamplesPage, self).__init__(cf, show_add_measurements=True) + def __init__(self, cf: Crazyflie, container: LhGeoInputContainer, parent=None): + super(RecordXYPlaneSamplesPage, self).__init__(cf, container, show_add_measurements=True) self.explanation_text.setText('Step 3. Put the Crazyflie somewhere in the XY-plane, but not on the X-axis.\n' + 'This position is used to map the the XY-plane to the floor.\n' + 'You can sample multiple positions to get a more precise definition.') @@ -258,13 +276,18 @@ def __init__(self, cf: Crazyflie, parent=None): pixmap = pixmap.scaledToWidth(PICTURE_WIDTH) self.explanation_picture.setPixmap(pixmap) + def store_sample(self, angles: LighthouseBsVectors) -> None: + # measurement = LhMeasurement(timestamp=now, base_station_id=bs_id, angles=angles) + self.container.append_xy_plane_sample(LhCfPoseSample(angles)) + super().store_sample(angles) + def get_samples(self): return self.recorded_angles_result class RecordXYZSpaceSamplesPage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, parent=None): - super(RecordXYZSpaceSamplesPage, self).__init__(cf) + def __init__(self, cf: Crazyflie, container: LhGeoInputContainer, parent=None): + super(RecordXYZSpaceSamplesPage, self).__init__(cf, container) self.explanation_text.setText('Step 4. Move the Crazyflie around, try to cover all of the flying space,\n' + 'make sure all the base stations are received.\n' + 'Avoid moving too fast, you can increase the record time if needed.\n') @@ -319,147 +342,7 @@ def _ready_single_sample_cb(self, bs_id: int, angles: LighthouseBsVectors): now = time.time() measurement = LhMeasurement(timestamp=now, base_station_id=bs_id, angles=angles) self.recorded_angles_result.append(measurement) + self.container.append_xyz_space_samples([measurement]) def get_samples(self): return self.recorded_angles_result - - -class EstimateGeometryThread(QtCore.QObject): - finished = QtCore.pyqtSignal() - failed = QtCore.pyqtSignal() - - def __init__(self, container: LhGeoInputContainer): - super(EstimateGeometryThread, self).__init__() - - self.container = container - self.bs_poses = {} - - def run(self): - try: - logger.info("Start estimation") - self.bs_poses = self._estimate_geometry(self.container) - logger.info("Estimation done") - self.finished.emit() - except Exception as ex: - logger.error("Estimation exception: " + str(ex)) - self.failed.emit() - - def get_poses(self): - return self.bs_poses - - def _estimate_geometry(self, container: LhGeoInputContainer) -> dict[int, Pose]: - """Estimate the geometry of the system based on samples recorded by a Crazyflie""" - - matched_samples = container.get_matched_samples() - logger.info("start initial guess") - initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples) - logger.info("initial guess done") - - scaled_initial_guess = LhGeoEstimationManager.align_and_scale_solution(container, initial_guess, REFERENCE_DIST) - for bs_id, pose in sorted(scaled_initial_guess.bs_poses.items()): - pos = pose.translation - logger.info(f' {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})') - - logger.info(f"Len cleaned samples: {len(cleaned_matched_samples)}") - - logger.info("Start solver") - solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions) - logger.info("Solver done") - logger.info(f"Success: {solution.success}") - if not solution.success: - raise Exception("No lighthouse base station geometry solution could be found!") - - scaled_solution = LhGeoEstimationManager.align_and_scale_solution(container, solution.poses, REFERENCE_DIST) - return scaled_solution.bs_poses - - -class EstimateBSGeometryPage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, origin_page: RecordOriginSamplePage, xaxis_page: RecordXAxisSamplePage, - xyplane_page: RecordXYPlaneSamplesPage, xyzspace_page: RecordXYZSpaceSamplesPage, parent=None): - - super(EstimateBSGeometryPage, self).__init__(cf) - self.explanation_text.setText('Step 5. Press the button to estimate the geometry and check the result.\n' + - 'If the positions of the base stations look reasonable, press finish to close ' + - 'the wizard,\n' + - 'if not restart the wizard.') - pixmap = QtGui.QPixmap(cfclient.module_path + "/ui/wizards/bslh_5.png") - pixmap = pixmap.scaledToWidth(640) - self.explanation_picture.setPixmap(pixmap) - self.start_action_button.setText('Estimate Geometry') - self.origin_page = origin_page - self.xaxis_page = xaxis_page - self.xyplane_page = xyplane_page - self.xyzspace_page = xyzspace_page - self.bs_poses = {} - - def _action_btn_clicked(self): - self.start_action_button.setDisabled(True) - self.status_text.setText(self.str_pad('Estimating geometry...')) - - container = LhGeoInputContainer(LhDeck4SensorPositions.positions) - container.set_origin_sample(self.origin_page.get_sample()) - container.set_x_axis_sample(self.xaxis_page.get_sample()) - container.set_xy_plane_samples(self.xyplane_page.get_samples()) - container.set_xyz_space_samples(self.xyzspace_page.get_samples()) - - # Enable to write to file. This can be used for debugging in examples/lighthouse/multi_bs_geometry_estimation.py - # found in the python lib - # import pickle - # with open('lh_geo_input_dump.pickle', 'wb') as handle: - # pickle.dump(container, handle, protocol=pickle.HIGHEST_PROTOCOL) - # with open('lh_geo_input_dump.pickle', 'rb') as handle: - # container = pickle.load(handle) - - - self.thread_estimator = QtCore.QThread() - self.worker = EstimateGeometryThread(container) - self.worker.moveToThread(self.thread_estimator) - self.thread_estimator.started.connect(self.worker.run) - self.worker.finished.connect(self.thread_estimator.quit) - self.worker.finished.connect(self._geometry_estimated_finished) - self.worker.failed.connect(self._geometry_estimated_failed) - self.worker.finished.connect(self.worker.deleteLater) - self.thread_estimator.finished.connect(self.thread_estimator.deleteLater) - self.thread_estimator.start() - - def _geometry_estimated_finished(self): - self.bs_poses = self.worker.get_poses() - self.start_action_button.setDisabled(False) - self.status_text.setText(self.str_pad('Geometry estimated! (X,Y,Z) in meters \n' + - self._print_base_stations_poses(self.bs_poses))) - self.is_done = True - self.completeChanged.emit() - - def _geometry_estimated_failed(self): - self.bs_poses = self.worker.get_poses() - self.status_text.setText(self.str_pad('Geometry estimate failed! \n' + - 'Hit Cancel to close the wizard and start again')) - - def _print_base_stations_poses(self, base_stations: dict[int, Pose]): - """Pretty print of base stations pose""" - bs_string = '' - for bs_id, pose in sorted(base_stations.items()): - pos = pose.translation - temp_string = f' {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})' - bs_string += '\n' + temp_string - - return bs_string - - def get_geometry(self): - geo_dict = {} - for bs_id, pose in self.bs_poses.items(): - geo = LighthouseBsGeometry() - geo.origin = pose.translation.tolist() - geo.rotation_matrix = pose.rot_matrix.tolist() - geo.valid = True - geo_dict[bs_id] = geo - - return geo_dict - - -if __name__ == '__main__': - import sys - app = QtWidgets.QApplication(sys.argv) - wizard = LighthouseBasestationGeometryWizard() - wizard.show() - sys.exit(app.exec()) From 2d900b01377fa07431d02fa8af8dcf5b1baf7b65 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 13 Jun 2025 15:58:01 +0200 Subject: [PATCH 06/18] Adapted to modifications in the lib --- .../ui/wizards/lighthouse_geo_bs_estimation_wizard.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index 658e2b7e..8f3024ef 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -38,9 +38,10 @@ from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors -from cflib.localization.lighthouse_types import LhDeck4SensorPositions, LhMeasurement, LhBsCfPoses +from cflib.localization.lighthouse_types import LhDeck4SensorPositions, LhMeasurement from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer, LhGeoEstimationManager +from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution from PyQt6 import QtCore, QtWidgets, QtGui @@ -100,10 +101,10 @@ def reset(self): self.setWindowTitle("Lighthouse Base Station Geometry Wizard") self.resize(WINDOW_STARTING_WIDTH, WINDOW_STARTING_HEIGHT) - def solution_handler(self, scaled_solution: LhBsCfPoses): + def solution_handler(self, solution: LighthouseGeometrySolution): """Upload the geometry to the Crazyflie""" geo_dict = {} - for bs_id, pose in scaled_solution.bs_poses.items(): + for bs_id, pose in solution.poses.bs_poses.items(): geo = LighthouseBsGeometry() geo.origin = pose.translation.tolist() geo.rotation_matrix = pose.rot_matrix.tolist() From e9c567763732177761d61f073da8264e373648be Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 17 Jun 2025 13:14:01 +0200 Subject: [PATCH 07/18] Stop solver thread --- .../ui/wizards/lighthouse_geo_bs_estimation_wizard.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index 8f3024ef..335f4ed4 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -67,7 +67,6 @@ def __init__(self, lighthouse_tab, ready_cb, parent=None, *args): self.cf = lighthouse_tab._helper.cf self.container = LhGeoInputContainer(LhDeck4SensorPositions.positions) self.solver_thread = LhGeoEstimationManager.SolverThread(self.container, is_done_cb=self.solution_handler) - self.solver_thread.start() self.ready_cb = ready_cb self.wizard_opened_first_time = True self.reset() @@ -113,6 +112,12 @@ def solution_handler(self, solution: LighthouseGeometrySolution): self.lighthouse_tab.write_and_store_geometry(geo_dict) + def showEvent(self, event): + self.solver_thread.start() + + def closeEvent(self, event): + self.solver_thread.stop() + class LighthouseBasestationGeometryWizardBasePage(QtWidgets.QWizardPage): From 77cdeca6611d7c8dd448bf6d8146f3e821e195d5 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 23 Jun 2025 11:59:12 +0200 Subject: [PATCH 08/18] Adaptations for new sampling method --- .../lighthouse_geo_bs_estimation_wizard.py | 88 ++++++++----------- 1 file changed, 37 insertions(+), 51 deletions(-) diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py index 335f4ed4..c99543f4 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -31,17 +31,17 @@ import cfclient import logging -import time from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader -from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader +from cflib.localization.lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors -from cflib.localization.lighthouse_types import LhDeck4SensorPositions, LhMeasurement +from cflib.localization.lighthouse_types import LhDeck4SensorPositions from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer, LhGeoEstimationManager from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution +from cflib.localization.user_action_detector import UserActionDetector from PyQt6 import QtCore, QtWidgets, QtGui @@ -101,7 +101,17 @@ def reset(self): self.resize(WINDOW_STARTING_WIDTH, WINDOW_STARTING_HEIGHT) def solution_handler(self, solution: LighthouseGeometrySolution): - """Upload the geometry to the Crazyflie""" + logger.info('Solution ready --------------------------------------') + logger.info(f'Converged: {solution.has_converged}') + logger.info(f'Progress info: {solution.progress_info}') + logger.info(f'Progress is ok: {solution.progress_is_ok}') + logger.info(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}') + logger.info(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}') + logger.info(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}') + logger.info(f'XYZ space: {solution.xyz_space_samples_info}') + logger.info(f'General info: {solution.general_failure_info}') + + # Upload the geometry to the Crazyflie geo_dict = {} for bs_id, pose in solution.poses.bs_poses.items(): geo = LighthouseBsGeometry() @@ -110,6 +120,7 @@ def solution_handler(self, solution: LighthouseGeometrySolution): geo.valid = True geo_dict[bs_id] = geo + logger.info('Uploading geometry to Crazyflie') self.lighthouse_tab.write_and_store_geometry(geo_dict) def showEvent(self, event): @@ -294,61 +305,36 @@ def get_samples(self): class RecordXYZSpaceSamplesPage(LighthouseBasestationGeometryWizardBasePage): def __init__(self, cf: Crazyflie, container: LhGeoInputContainer, parent=None): super(RecordXYZSpaceSamplesPage, self).__init__(cf, container) - self.explanation_text.setText('Step 4. Move the Crazyflie around, try to cover all of the flying space,\n' + - 'make sure all the base stations are received.\n' + - 'Avoid moving too fast, you can increase the record time if needed.\n') + self.explanation_text.setText('Step 4. Sample points in the space that will be used.\n' + + 'Make sure all the base stations are received, you need at least two base \n' + + 'stations in each sample. Sample by rotating the Crazyflie quickly \n' + + 'left-right around the Z-axis and then holding it still for a second.\n') pixmap = QtGui.QPixmap(cfclient.module_path + "/ui/wizards/bslh_4.png") pixmap = pixmap.scaledToWidth(PICTURE_WIDTH) self.explanation_picture.setPixmap(pixmap) - self.record_timer = QtCore.QTimer() - self.record_timer.timeout.connect(self._record_timer_cb) - self.record_time_total = DEFAULT_RECORD_TIME - self.record_time_current = 0 - self.reader = LighthouseSweepAngleReader(self.cf, self._ready_single_sample_cb) - - def extra_layout_field(self): - h_box = QtWidgets.QHBoxLayout() - self.seconds_explanation_text = QtWidgets.QLabel() - self.fill_record_times_line_edit = QtWidgets.QLineEdit(str(DEFAULT_RECORD_TIME)) - self.seconds_explanation_text.setText('Enter the number of seconds you want to record:') - h_box.addStretch() - h_box.addWidget(self.seconds_explanation_text) - h_box.addWidget(self.fill_record_times_line_edit) - h_box.addStretch() - self.layout.addLayout(h_box) - - def _record_timer_cb(self): - self.record_time_current += 1 - self.status_text.setText(self.str_pad('Collecting sweep angles...' + - f' seconds remaining: {self.record_time_total-self.record_time_current}')) - - if self.record_time_current == self.record_time_total: - self.reader.stop() - self.status_text.setText(self.str_pad( - 'Recording Done!'+f' Got {len(self.recorded_angles_result)} samples!')) - self.start_action_button.setText("Restart measurements") - self.start_action_button.setDisabled(False) - self.is_done = True - self.completeChanged.emit() - self.record_timer.stop() + self.reader = LighthouseMatchedSweepAngleReader(self.cf, self._ready_single_sample_cb) + self.detector = UserActionDetector(self.cf, cb=self.user_action_cb) def _action_btn_clicked(self): - self.is_done = False - self.reader.start() - self.record_time_current = 0 - self.record_time_total = int(self.fill_record_times_line_edit.text()) - self.record_timer.start(1000) - self.status_text.setText(self.str_pad('Collecting sweep angles...' + - f' seconds remaining: {self.record_time_total}')) - + self.is_done = True self.start_action_button.setDisabled(True) + self.detector.start() + + def user_action_cb(self): + self.reader.start() - def _ready_single_sample_cb(self, bs_id: int, angles: LighthouseBsVectors): - now = time.time() - measurement = LhMeasurement(timestamp=now, base_station_id=bs_id, angles=angles) - self.recorded_angles_result.append(measurement) - self.container.append_xyz_space_samples([measurement]) + def _ready_single_sample_cb(self, sample: LhCfPoseSample): + self.container.append_xyz_space_samples([sample]) def get_samples(self): return self.recorded_angles_result + + def _stop_all(self): + self.reader.stop() + if self.detector is not None: + self.detector.stop() + + def cleanupPage(self): + self._stop_all() + super().cleanupPage() From 5cd63e21d3e6621dcbaddc41d371bedc433c3fdd Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 23 Jun 2025 16:40:03 +0200 Subject: [PATCH 09/18] First step of integrating geo wizard in lighthouse tab --- src/cfclient/ui/tabs/lighthouse_tab.py | 22 + src/cfclient/ui/tabs/lighthouse_tab.ui | 776 +++++++++++++------------ 2 files changed, 430 insertions(+), 368 deletions(-) diff --git a/src/cfclient/ui/tabs/lighthouse_tab.py b/src/cfclient/ui/tabs/lighthouse_tab.py index 4407dc4a..c2f0e582 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.py +++ b/src/cfclient/ui/tabs/lighthouse_tab.py @@ -31,6 +31,7 @@ """ import logging +from enum import Enum from PyQt6 import uic from PyQt6.QtCore import Qt, pyqtSignal, QTimer @@ -262,6 +263,10 @@ def _mix(self, col1, col2, mix): return col1 * mix + col2 * (1.0 - mix) +class UiMode(Enum): + flying = 1 + geo_estimation = 2 + class LighthouseTab(TabToolbox, lighthouse_tab_class): """Tab for plotting Lighthouse data""" @@ -357,6 +362,9 @@ def __init__(self, helper): self._load_sys_config_button.clicked.connect(self._load_sys_config_button_clicked) self._save_sys_config_button.clicked.connect(self._save_sys_config_button_clicked) + self._ui_mode = UiMode.flying + self._geo_mode_button.toggled.connect(lambda enabled: self._change_ui_mode(enabled)) + self._is_connected = False self._update_ui() @@ -390,6 +398,7 @@ def _connected(self, link_uri): logger.debug("Crazyflie connected to {}".format(link_uri)) self._basestation_geometry_dialog.reset() + self._flying_mode_button.setChecked(True) self._is_connected = True if self._helper.cf.param.get_value('deck.bcLighthouse4') == '1': @@ -485,6 +494,7 @@ def _disconnected(self, link_uri): self._update_graphics() self._plot_3d.clear() self._basestation_geometry_dialog.close() + self._flying_mode_button.setChecked(True) self.is_lighthouse_deck_active = False self._is_connected = False self._update_ui() @@ -519,6 +529,14 @@ def _logging_error(self, log_conf, msg): "Error when using log config", " [{0}]: {1}".format(log_conf.name, msg)) + def _change_ui_mode(self, is_geo_mode: bool): + if is_geo_mode: + self._ui_mode = UiMode.geo_estimation + else: + self._ui_mode = UiMode.flying + + self._update_ui() + def _update_graphics(self): if self.is_visible() and self.is_lighthouse_deck_active: self._plot_3d.update_cf_pose(self._helper.pose_logger.position, @@ -536,6 +554,10 @@ def _update_ui(self): self._load_sys_config_button.setEnabled(enabled) self._save_sys_config_button.setEnabled(enabled) + self._mode_group.setEnabled(enabled) + self._geometry_area.setEnabled(enabled) + self._geometry_area.setVisible(self._ui_mode == UiMode.geo_estimation) + def _update_position_label(self, position): if len(position) == 3: coordinate = "({:0.2f}, {:0.2f}, {:0.2f})".format( diff --git a/src/cfclient/ui/tabs/lighthouse_tab.ui b/src/cfclient/ui/tabs/lighthouse_tab.ui index 71f5cce5..6346f80e 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.ui +++ b/src/cfclient/ui/tabs/lighthouse_tab.ui @@ -6,399 +6,411 @@ 0 0 - 1753 - 763 + 1302 + 742 Plot - + - - - 0 + + + QLayout::SetDefaultConstraint - - - - 6 + + + + + 0 + 0 + - - QLayout::SetDefaultConstraint + + + 0 + 0 + - - - - QLayout::SetDefaultConstraint - - - - - - 0 - 0 - - - - - 0 - 0 - - - - Crazyflie status - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - + + Crazyflie status + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + + + - - - - - - - Status: - - - - - - - - 0 - 0 - - - - - 200 - 0 - - - - - - - - true - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - Position: - - - - - - - - 150 - 0 - - - - QFrame::NoFrame - - - (0.0 , 0.0 , 0.0) - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - + + + Status: + + - + + + + 0 + 0 + + + + + 200 + 0 + + + + - + + + true + + + + + - Qt::Vertical + Qt::Horizontal - 0 + 40 20 - - - - - - true - - - - 0 - 0 - - - - - 0 - 0 - - - - Basestation Status - - + + + - - - - - - - QLayout::SetMinimumSize - - - 2 - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - - 30 - 0 - - - - 1 - - - - - - - - 100 - 0 - - - - Geometry - - - - - - - Receiving - - - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - - 100 - 0 - - - - Estimator - - - - - - - Calibration - - - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - - - - - - - - - + + + Position: + + + + + + + + 150 + 0 + + + + QFrame::NoFrame + + + (0.0 , 0.0 , 0.0) + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - System Management - - - - QLayout::SetDefaultConstraint + + + + + + + + + + true + + + + 0 + 0 + + + + + 0 + 0 + + + + Basestation Status + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + + + QLayout::SetMinimumSize + + + 2 + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + + 30 + 0 + + + + 1 + + + + + + + + 100 + 0 + + + + Geometry + + + + + + + Receiving + + + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + + 100 + 0 + + + Estimator + + + + + + + Calibration + + + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + System Management + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + QLayout::SetDefaultConstraint + + + + + - - - - - - - Manage geometry - - - - - - - Change system type - - - - - - - Set BS channel - - - - - - - - - - - Save system config - - - - - - - Load system config - - - - - - - - - Qt::Vertical - - - - 20 - 5 - - - - - + + + Manage geometry + + + + + + + Change system type + + + + + + + Set BS channel + + - - + + + + + + + Save system config + + + + + + + Load system config + + + + + + + + + + + + + + + 0 + 0 + + + + Mode + + + + + + Flying + + + true + + + + + + + Geometry + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + - + Qt::Horizontal @@ -414,12 +426,40 @@ - - - - QLayout::SetMaximumSize + + + + QFrame::StyledPanel - + + QFrame::Raised + + + + + + + + + TextLabel + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + From 96ddf1cc923bc15f452c3b2f86f235abdad1c665 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 24 Jun 2025 16:37:22 +0200 Subject: [PATCH 10/18] Basic functionality in geo wizard widget --- src/cfclient/ui/tabs/lighthouse_tab.py | 9 +- src/cfclient/ui/tabs/lighthouse_tab.ui | 25 +- src/cfclient/ui/widgets/geo_estimator.ui | 133 +++++++ .../geo_estimator_resources/bslh_1.png | Bin 0 -> 6677 bytes .../geo_estimator_resources/bslh_2.png | Bin 0 -> 9105 bytes .../geo_estimator_resources/bslh_3.png | Bin 0 -> 8838 bytes .../geo_estimator_resources/bslh_4.png | Bin 0 -> 13664 bytes .../geo_estimator_resources/bslh_5.png | Bin 0 -> 7743 bytes .../ui/widgets/geo_estimator_widget.py | 340 ++++++++++++++++++ 9 files changed, 483 insertions(+), 24 deletions(-) create mode 100644 src/cfclient/ui/widgets/geo_estimator.ui create mode 100644 src/cfclient/ui/widgets/geo_estimator_resources/bslh_1.png create mode 100644 src/cfclient/ui/widgets/geo_estimator_resources/bslh_2.png create mode 100644 src/cfclient/ui/widgets/geo_estimator_resources/bslh_3.png create mode 100644 src/cfclient/ui/widgets/geo_estimator_resources/bslh_4.png create mode 100644 src/cfclient/ui/widgets/geo_estimator_resources/bslh_5.png create mode 100644 src/cfclient/ui/widgets/geo_estimator_widget.py diff --git a/src/cfclient/ui/tabs/lighthouse_tab.py b/src/cfclient/ui/tabs/lighthouse_tab.py index c2f0e582..6af2430c 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.py +++ b/src/cfclient/ui/tabs/lighthouse_tab.py @@ -42,6 +42,7 @@ import cfclient from cfclient.ui.tab_toolbox import TabToolbox +from cfclient.ui.widgets.geo_estimator_widget import GeoEstimatorWidget from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import LighthouseMemHelper from cflib.localization import LighthouseConfigWriter @@ -302,6 +303,10 @@ def __init__(self, helper): super(LighthouseTab, self).__init__(helper, 'Lighthouse Positioning') self.setupUi(self) + self._geo_estimator_widget = GeoEstimatorWidget(self) + self._geometry_area.addWidget(self._geo_estimator_widget) + + # Always wrap callbacks from Crazyflie API though QT Signal/Slots # to avoid manipulating the UI when rendering it self._connected_signal.connect(self._connected) @@ -555,8 +560,8 @@ def _update_ui(self): self._save_sys_config_button.setEnabled(enabled) self._mode_group.setEnabled(enabled) - self._geometry_area.setEnabled(enabled) - self._geometry_area.setVisible(self._ui_mode == UiMode.geo_estimation) + + self._geo_estimator_widget.setVisible(self._ui_mode == UiMode.geo_estimation and enabled) def _update_position_label(self, position): if len(position) == 3: diff --git a/src/cfclient/ui/tabs/lighthouse_tab.ui b/src/cfclient/ui/tabs/lighthouse_tab.ui index 6346f80e..fa9a0687 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.ui +++ b/src/cfclient/ui/tabs/lighthouse_tab.ui @@ -404,9 +404,9 @@ - + - + @@ -427,26 +427,7 @@ - - - QFrame::StyledPanel - - - QFrame::Raised - - - - - - - - - TextLabel - - - - - + diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui new file mode 100644 index 00000000..003bdc5b --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -0,0 +1,133 @@ + + + Form + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + true + + + + Geometry estimator + + + + + + + Sample collection + + + + + + TextLabel + + + + + + + Image + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + + 0 + 0 + + + + Start measurement + + + + + + + Qt::Horizontal + + + + + + + + + + 0 + 0 + + + + Previous + + + + + + + + 0 + 0 + + + + Next + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_resources/bslh_1.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_1.png new file mode 100644 index 0000000000000000000000000000000000000000..0b1d1f75443558966c82c9b5ae35857148bf29b2 GIT binary patch literal 6677 zcmeHM`8(9z`+pNPvZj(`O(hM+QplQYp?GM)j5Q$*V@o63kiD@yPh`m!Qi>rl#yT?$ zMwWyc>sXp(8Qahx!_3$7`6s@=JU^WKy3RS*b$+<7*Zq3k_v}P<={UY{8tA5qXKPH3^O5;hj`%_ zFw29btl0JJL+Kh@$AP@-KwJh@rOXLjHugTuaVwk#RnFE%J)!BB1 zkg}HBmVchWSHP^SiZ=ut3@$vc`JS>H5ivnvS7vS?;46n9vcA$>B3|F1;n{298mQe2 z$~gPk#NO-74QNCtehA9yz|UPuKAsnA*JBM|2^f6K6kE3IvBzNTTcSVa@?6VFawHI- zA6=#EC|=&YBhVt{vc0Cv4Nv&W#1`1Tf1%?S0`7-IZ2mC$rK~*qaV&)^j)o}uXj4T_ zT*zm){dwxoRf)xW5`tcKB=@`c4xLdW?I#%ssU|>LRQB4uhPgrVjN+X|Mb=MS#ksCf z$-5XVa(~N029jl$AUt>q26r7NIPqdBy81h*nsePbl!t9>CF}cBQc-LA0(lld+&@sb z%&Cyb@Oh*O922JRz6pvJDM9<{ zs4SV)y=pO}+F-6=xg7bi6axmWSz{us^2(WRBOva$fU*MA$id!rU_(K{m(8;Fl9k~S zv6clseG&Uso4+|};praT?;)m2{51|cyZ7?87yi(h1!QYEkf#9Ugz;~+e$JqKa1kz+ zrI7dahsnf8ym5Lw#M3pu=+=s5s7SBJsVrMl&W!qzTjZZ{XKM6GPb7654xu~L9#v6X zWwvWapt>J7Wg%Pv=eoj~Re$I4R}Bs1Pp@jsKf>&X;UJelU1KblKFb1SD?51PUzUx* z|4eWJzS0@j@O&`+8?8FK%+iM|%`%35O7dn=~AgOYC_J9Dff$^{TF~!Hyb*PC!qvd;H1YCC$3S6rod1MI8-o z3hI2Q?UKu+)ydp2jA8#ZY*=Z% z+Z0ICG`5<-G}r*q9_lz?HSL09UwsZ!&A1v{ZK%1!C)9a4RM zfFa4RS~<)b0LBivC9hIEoG~>~UYz}Rfr}aXLia_xzkLl0jqI$6Ce<(7;K~eLlzy@| z-C>U~!XB)&qzMBbab7^v=@+1Vb5C3INj0VEIQpGU@5kSaXRP7X?+ZtzE$fyuHIj(h znITt)1odAP?@>SNyP%U5f-x~R5@uY{g4ybT^|N6oK|)5?2GvPq&Enjr>~8`St8P zJ%JUfs_~u&kPQJOy}o`CyM@3|lLI3mrs5IBnH!0vh!K|rPc0}XVPegk{=-p_zP)P3 zSfpMwzWsy24ycpbr>BnMwE7L2`iq)o)~N*-oEGoOuOEOb#uuE2LE2=e*7Dh zWyC3S=mg!k#8R4RwcVz-DfU)IDz`fjrXWA;QI{u2KE!|#T6H(7B-pDaJ9Gp;3Lu_+ ze`w}npZ;?S4&OJ9Id4hw?XMKwc=I-9r8=vTu%JFAgMF}7wqkwhfgo@$yNN$kM8kXA zyaJ*3yh!*$n{T*RNE+vW&0}}i{f#wGt@PNTebd!eiqjH)<^+Q7@ap2|FKUB;dtIU- z94L}^Ci+S^6R#Zu2*)r{t#ivO=lwTKAInV_rSR$92Ic3j*GO%%rux3Gb&mM-6U_%# zD(2Sm`J#O#xI`{;&jhN;c_^q1zh=s|PZcoQg1zo{+sOr}> zSvF4s$UeQ+RFY^@yZ802LG|W2fZG(%GxQdWQHUqx(h;2vB=^Qf>8;g@O(a888*uq% z$Zn;K--gA8-7Xq=DhkA`BCCm-zH!o1XO#rxK92Ay@h>rzrPw`Moh9-5z>E_pdj4Uq zNGN3;nOjm8n8UT-*QfKjqCQ$O?ZU90Wc--1^N0=klk%$r9v45#VC$r)QNPXe22Ak~ zgP+)eA|z)0Pb_uCtag>_{(194bNhnfbUJ{%H&IG%Rx326ZukGJSxvwGqZompI131m zK?Cfrgv@k+8U(m!cpXHzIR~m^LmT{ZelPWKi?ECe%}uL-sIIq4Y7n5IR#Tw!D$H1K zBW-G0(KL9Prz89rh)@`~Li(rT5?g`n@#GyRaX8~c4THrVEN>^S;|ZeryhJY%0MdOE zq<4DD6)(Yc3=BYUvjISK04lcOvJxqQ~Hsr3tE^QTYQhE9`g~` zz5~qZb+UpbcM|yANE}*Qz_XAnzEhyl08Wbb*(k|+9SZ4aK zXE&7Qo_LI#N+jHi^=W7(d@lC7{AVsuTQYJ1L5^C8S#P^2AkF{6z2Zm^t<|Bk{0uU5 zO;Ev|mhn&P=1x(*_3m@>RPyoHc&M$R^yI672Y=DZR{pPInRfZc=MDFYgBIg{}Mq#Tiq(aHSz1QQ~U@ac!-5-@^SKbu+9s&Kwi& zngZQY@zlP~(^GBF^V}sKRw~<|6=oQYI%IYHAV+<1>GoENGLTnNlh6srjCrP`;#@Cf zoX*(nEz2x=Y3JgLiHsVX&^Q{0Gc#>rCH{0xBf0U=;iSa!RktkFx?L%gg|H>sddYYH z@Dn^J7x=!r;vA}j6|Qw1OiYdfRE4*4|AOOmL9l;0g9!9iIgw)uXpjhS#SF zsfH_N*=oe(|Kaa__D<8P+?vO`gQ|Bt=)66x55kuX8nWR)@>n)^OzIVilj&?AAT1S(e*YEvIh|0)HWHWFT1&ro&i3K< z@~%OI2URnSk3Q)fW|$`IdD$iKqLoIRakL{Ai-zh(wwkOrV}B-2mgdBV>9$W=f+I}2 zb;Uz$@u$HFN;yy*1IX|b4GCkV zv@j=9F0&%Ab^AV%NkCJ|f+!~q#Oiq1XuBT%(uzpQi6HR_^~yrEI-xDUr_Cr=IJL1u zRHP`=&SmP``l+?#g5nC*XJ6faTyNRddfzp9ag@CV{&;oXnjg7nv{nnH!vjmkA=OSX zE6>34kwAXkl$6g{o)h!6q`zVd{?X2vzBr=X9#g`#S>wotT*R_D{|^i(Iua&S((ySy zfe58Ka+rIse0 z9qgU+ZBb%WF!=)NceEr1T$FZrygGjB**N`!snv9FYyQm^nFK50M2IQ>@;(Sn{Qlc0 zjxfb!C69p8EYAS)=L9oUq)m5VALf*xg43t>m;5`mTkhh@jtsg>4pr0WL+9_`??e~> zCiU}6?@EeRwF?=}a=?$V;inr{oUDjN@a{tXs7SA~X@Z%ADW_6Y=}p?wHt2eqG& z9}Jl9KnY?huy;BhqC9fQf4XW^q4$deKIpzHJ}@h5_@P`&>4J*=Sh**=LVS8MT^VVm zh|-!`Bk0VA27ND@@C{A9`#efg|6~!FkDkrYm3@=XBkF-mpb$K$p@FJ;`EjrEGI(dn z#|?I4n52`&BLR8l#!|%GWI2>h6PpPF!-lW;(Vie_Yq~Yg^D06uxOYrG?_~p{iUrTz z){R@>rDF`2QJCtXCx%d1k?rH`ino;U^phM7O*%`hDzqwUamB%PN4)}+pXl1D(~Y}8 zZj*jOxPs?U#?kQKEi-lx3V8JdrciaS-;h^sL+jWPL9$Y-G^lX4w_cJE*JAe*lN{Ej zT@T77pgFImVE7(678HHzs?eH4;1X{=<-ytRwN$8+%$e^#EyqH>r!CXjQ^_Thu;CHm zs%Vab<)4Yc;mv^6bLW@R*nxXC)47Frs$y_7liEZg|JB*}TQ9EwLd6ek|I)>X|2`mY zj{4r$L#{kApRU(T&h7?JY-}jv3gCe)>qF=EufgC{*OHR~{!)*Ab!y`qt0vC`Hp(bh znL~Hgm71m%I-Fwq8$jU<0`8x(h!W2=nFB%&RGgbuf(EHiu{dusH>!7&S6)@g4%tu) zQ|3SA)mba#J1(iK+RS|FLFI30+xSg7XOhN}KV2k42Pij%Lr31ls6d0lfv|?xU9=~E zyu4K5ZwU4KArmvuv~V^2A3|!^37OnGV$&#bu*g1LyBIdFh8Zcfo^N|=jfmOI#Ebmq zPPgW>KgEbK9C`JalUX%se(Ix?x9F@lJbsQ|^`e*=;;nPx^7WaLngZQ3B2ihl$*?KG{cyiI)uj9AXH=#R{>+87!oxp@pY=yRE+BytWf0xgVG!$~6TXS39h;+Na5J4t!nbkO567Mc zznyrfon!zlw(`*)#$R2jPhvq&sNx+&fW-OWMe2*vmllQSvSrF=)eFlrbv2O;9j5R| zc(6v|?4{?&^9<21(NU;2wUX^i5d`eL>fO0a=gA^6t0qiXRo)Xwal2CYx73+UQ6n#K z`$%iUZ?TeWufw*68b_U3!z^e!yBv%}z*al7o107h7Udqp-@YMjmEY;VG!dk7KVoJ} zCp8rv*(jA^ASCv(Iyvz6b}9c#J6yT_s8BDXFnQL2xmJ3)4p2E{^0^p( zmbBp>;kN3(-e;?6l7DA4)zU?(V;e?dA%li?yqq}f59FF&&7;x6XLt0mF2WAxV~1w` zB5sd68qb{D(@#1?@A2**oZIo)YxrH?VHAP7Ts)VkCM100(PO)(suEQJ%Q@O^dTf<9o1rmD_* zjArR7R3fidGM;K5MIqx2-7ddnrk8rp6cq+s|6|W!4p1sXnu{J!i?ZFWyoUit26yWX z-@9)!`rH-@4hC>hkB%o_HeWZo!SG3bS5tC7z%HL3*e?rVXiBV6c$E?b^?!kz-KS zR+oVf1uEcpx~oozoEaYY>8Uwb;-@@dozm0fn@)KaDV|Nr;R*2Z?y-Fr09G*``Q7#q zCfQK*>_C$lH&iC< zQ_~MdEgwwlLT7yci!f1;UNV#}T*A8^qWdCb1j=g39D-D*7Fu%H?_8Wl^&CgI6BI;g zz_emn%YlW~GxfkY;*RB?Kn!!t&JvYw2nO1{hIj?MPspSdnZiFelQ94KSlN}eq5hl$ z^Q=`4keCAUEcj*fsjxvNYwu0-avkr|tR)>YhNYyNtufY~cDiZ!aPiF43_keVpm7nl zCE$A@;~EgEbRjn4B#kRZ@I1i1?tozy?6^2vjkkR<^Uk#OvOdN y-^u#idHv@^4lK$PE*{{98SwuXx(~=(%l>53pIoV`zYqR-01H!Vld9`(&;AcZZYhrd literal 0 HcmV?d00001 diff --git a/src/cfclient/ui/widgets/geo_estimator_resources/bslh_2.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_2.png new file mode 100644 index 0000000000000000000000000000000000000000..56dfb5fb66c7038b8af662773b4e0121e44694d5 GIT binary patch literal 9105 zcmd6NXH-*7)b0s26a_^DktQfbx`Kd6Q;LKp9qB~@=~a3X1f_$%fOJq0RJyd#0s$!^ zy>~(p0t5(134{c0-h2Puwch*huJ!$xb=K^eIcxUbXZCsaGduQ~p5`Tb4tf9pE@?eg zGXww%H~@fDz%*ou=)l0g19aX`pZfs-!_|KqXmkNe1OQ$@OHI`{C}(FuKjsT2muC+% zlQAGJ|MZ^x?PxWz;7WUo`diWOzfqflwH|o#i2jpeLG|bTz{>Hh=_u+t&j(cV4q7OO z$PjbiUmPpfBRH!CwzPN~r@zrMHg+27`y!m|dOJG)MmZBhdvN2%0cK`_CP>^_n9{RD zfBk?XOD}Q{Pg1<_6}|%khLda$EGU6mb$|+RNdUmWFgu7D_#{Qa3#d3!sQ~b9Fj=L< zD*y=K;0GuGX^sEE6PK^bV~`;=$w0CjCx&F8Ib#SsZ3Mu~ESj+U26g2;T-|(-iGOQr`S(JdZ8ufp>tb^5e zF!cBeV8r*I-YcX2*A}e*4`-WIU6%xm{xAP$3c~u19joG zjtUApdQoaxMR^PdG06xrG%zE9OEbK@BLQ^l0a46_;mS=8FmMErkOzQ2KR75(wXt%5 zwS19hhfCcDNA;G56MP4IKx60;Ji)MXHjuf! z^1(r#jcIQDoyFJdQ60hfg;+^*lBw_-Q;QLhL9dHSfG}=+YxnZ9VZpuR;}j1?$9vX# zN#CSIy#T`#9+!=TGMhBAgzp1(0a@@HwP2{`k3`J?CFM$XU_ODKH}dug2t@$H86k)J z3P~d^Z+L*R2;diQ=eM~Ob7%asJxchI6ceu{>%j?E&&GUBMzQ({C0xc3^unzmAMHcc zwvZ7*4Vd2ty-)0u$7$@%#hNG#w$KAr@>%eiZ#jNkGiN^Kgf3r?NG6z+BsC!Lg!+AA zOo{Cj0+;sE3BE7x{$!P%D}WZtIDK6@oV|S9jm7-lkKjgwH9Aqv!<3&pzWpB#LsZfj@<55Sl% zVCN~SamrnsdKn`8^;igC%m#k_4k3}Y)Otf{LI9K;7+!X}!&VmMXqF-YBt$F0tt`v7 z3`)Z>QeE+Tv;aRJ;460J+;t82CDnJhW3lG{eD>9~?3iSLL-5?m+)I3Io}37z2&Q62#1Pvht6C7q^y~fZ|>YRR0|!2<87B_{;dO zKo|HUOx~_eKJbf${H#?4{`@dsUX@!vWjP1zI;d0#ea)GFZU7v-zyom%F)XT$f7wgP z4MZs^Lt*zbWEoeR>N`(32jHe)xJM~#TXa!s5V?2?Ug10dBJJLzSNjBTXTVF5xghCBBS${yqDl0p>*E!3HHTdk1)M z8)KzOv|&F40Ur118?MM>3R4Zs0q*)%3E)!z0eHYGa8D^g26~A={ylsL^sElg6zaMq zfZ-8Z@0G(dO2FwAMP60T!DlE;)+~FNd_Emv+~l*h7~8v~aAf{N!4DDN5WwxK1%Jg1 z2;N;kJA*Y|XdG!!JxnpMCBlci zNQgk12!Nsk>Mnc4#4Xy)Z&5Ghf?rw7DeO|rc|_HM)4${KuI6Df7^2Ym;&Z%-P}DMc zb8j4S(M|o?0Z^|90lRlX;)ln4K|AV26+BJ#fAbP~o8}naRcbIRVm&Etx#t6NQa&b7 z4$!cn7eWPiA;fpM)3Z~0rECjB^8Yn)E+-ywAPwSxCYwU0AICcX0RE2Fx{B{*`3RiWcIjpX^3J(s<^K9+Ov z%1iw5QWkk;1viyru)qMnB!$n&Yfk%hxsvCP(K2c#dsVV3>)+mW8n{1Vi~{5wkn$I= zb5!i6Lq4zNrjCYQu?X|@5+cxWA8Rl(j8{?VqE=t8+^&XZWj1`z=)pZIfl73#(B{us z4!dzH#gIZj_$kGqE{XC2Nyd!CAeRnj_6m;21=$)%^U}{=LrZUuz7-ZZULFruWv&na zlAdu6GZNvT_!8Sop-hF@-QTC&NFl1;vKp5d-bh9Ra(bF`R@+?WWoAdE6Q`t%K7Bvj zy9tOJW73?-l_+OAI6n(Nxa~WE_9qKTy;)$UV-Af2}WC!`YlynU`@wck4Av>S{Hp zy{(e^eW-t4VS9-g5ezgtxtW}yRQwL#gO%v{&8JX~W1GDNB&p@Nye@Qye(ZE zIP%r~3q*L~VL#iC&YTE7X;qfJr=3}!oqeh(>V+_2S1IXTQ*ez_4jjIN3+l&7Yik(~ zLEcn{q)7Xi-_NGZ+=nneSq_(bR=?46)1T^m5LogV#xvE6mOPOG=V-!|^Lt|Bw3-9? zw_#<^GlZ`Kw0h{a9de{2$31qr*xt994IF!geFNGf^Dyo-)eI_-_lfa^UA#ln`x+Pe zaax2FGWHAB4dzfQX><#?oUz5@ZWh(H3Wm{8NsHQC})5@pnuLRm`S(ftzU`_ak>(eQT!~OY)V`fKw;If(G zr<{`Wb|*TRXrb)WKWXNcCy!_t+J!P`z2Oc`5kp%W_qDa#*TJx|yCraPuwIlgkC9>q zXoW7{4#<--x{P`UhRqur1I_0Jw2PXInF34p56MP&pn0vWPd;5{8=fnGk8Mll; zoEdUgxVlxT%g#yI{zyb>* zC97!UF{>vzHxhd%)l>FVt%&*EXDkmi0M2($u;(3wPJfA`dYoVPdo|^83($Q0QB+`y ziu;>ab~L=0dEILvlZ00I++_Cd8mk{k%6NorPwwvc*&uCMHvQ7afNE+`in(&r8 zM5z~kSy72soyv^?geYb{0D^G3JzqWh<$59Z62VToz?F0||7nf_TR_neex`BJv4ed$ z!2nAb=ohkQ(X6>nv2?xbuL_flZu`y_6QRoOMxjlLx2#Vs!wt_gfhDoHb+jMLXvfZ` z*g-MBV#;u+zNL1CB|qS4FxUu68PC4E z&QGFfqyu?}CY*Cc&nh2CsK1Pjh?`*1b9fw}>!@Q<#{CzfsHAS|+wcJ7%~0?JcaQ~( z7OSYhb!tEls=u@Q4k{1HUl`!d@(v?f&pN3o82SY;0VB=zIu3?CL(P82wepTR44%4dIH%N1o zy6WfDzpJUEY%9kh#1qmj4+Z_~F^S=@2+;wdC9z-f`Ra9#H=&;#ZWITuyDv;#H_7xJ zM3>)^>_B6E9=}X1I|x-oe)y7}#8=L`ojU4VlsaWbG;%k$0&{gqOAU%yX#w+ZW;_2e z3~9I0`+1Gjt7Ki}!vqmlI5_9W5FbM}*@TPqA;PvU?g$Tc(Icnd>35FA-FL+;Zru@o zu~?CYGW_lQB#gVCjqs24qFE%3k}M@)XMn zL;btD72~G%v|=j5FAO08!#(LnkOi&7zM@opj`>8lubX%RA9KshSyyOLs^BfjyVJ_C z@fkROWjr@`ex_C-{u*b7CJ>dE7JW=OZg+%=_1yFDA4 zADJnKY;ygEoJ(eOw8LyJmnRO0l5@}z6FpIGV)|o?!jj)xyb_?2#(+%pSzdLrF7%MMouKEf1d-|U3tmaiGueF|fN z28EBb6ZgSC)dvLRF`7Om;kCP^Yc79hldDPFWVVpWF6Q&!{O(j9>yuJ@oXtFU*HB*kSy)JK9JB1gM`T9wuI4^seLy=< zP%kb(LGQZQ-eneVPJR>X4U|}1qYgHBTU5N(<>>({3aoc@ccXN_P}8SFK2ve|a6K0L z(6o(3H|h*w@;JG;!s{67uUuaV3pfr#%?O;j*I-{@St$xJ8`+c$yh^ zBo3CRyZ6BPCf&AetgbNrOH-?lLo z{yZz9-Iz`y1lq>#;jL2t%Fx~q+0Sh9zW3%R{y_IqM9;|8u1*_vp(8D?O^QH31NxWOw4%NAFH5o(0XG08OS%!DiCMx#Z99IK&SO$dtyE=w|G*5WeU zdiY`H)b>q&A2&bO*g19or!WX{cfa4UqDgE>XVXyRmeg;&y?f73^+Kqd^Wug=2H#T0 zhMGE2R|JZB-5FfY5P@l8)ip{1NtQS+jNcuyT2>oA6LRXujh|h(sM@x9){`(5mcj9D z6S@k1_YPnS2|J|kubK?n;t7O+Ifo9b?h4cYO(}JglXXZn-It-|~SiAp==P zxvgOEImlsMs$%YNcH!bYhh@>eA1^zUOKs9B4zzAi_Dy$EkGGQXhApG?BW8-P-zR`+ zlOFE=^B$c5?+?McNc;+RkhQJ(SIbC`naO_M=|!fJ5dn1XLC*+l$EdL+Gm<%|0(RZDYUA+tTeC|c^FnWeUsevEWx&HchtXW_#IIZ<1fos6+0rcpf3(E-;llQ@q zW~SvT9raw_&)dDpTt*@G1G5~coCYO$OQPhmzM67NC94!Y#Le5~-f?Wcw zi0IGBx3w68y)?!Ofen;6MLvZ5^&cE+K97yB=XtmB?1-t`UVtjg$Ki`b9&J@=9#Y=f z828gWakbohcjsrui_^dr@xUhL>FgR&v2VYu?@~CjY49Y6CG6o{OGynv78z1Dl8|^W#~O$EC-v2M?As4PS&m=A6iXo)}6s>?ZwWU#PVG{ zAJIg!l%^u6zoPf`#9rXIWs12G2jIqrsSNoXR(4fRTuc@e=r!NGzmxsr&%nJjRLB|; z7K*-Dxt?Wp;fO=S@(c+`x2QG(0v+o`rjm3D7*HCAlT6nsG{0;D8BiI70UL#zI&Rh6 z4WHSYhaKW`56?0rxFIhHL|rGXWTyV+eUD2p*sY?&dwze}_^JI3w zl#eP&za|`TR$o%sEN=`=(;V?3E$if?in2Y%h)Qqk=uViN*M(Ym4j5h=_4~7|A0v)D z58Hqjx4^}xSbu3RyyA#!_`6X_r_gLI9^DjuRK|Af5wJZdWOyby@Z}tVR*~CpR-EX(QD9 z?6WLVmW}hL?n#4cUS&l_Yf*b&x^7H?))yvapSY3P4c%vSoB|+k-SsGyWhS@)UBu5Z zqS`R_n%5p|>Wl?uk?uw&JzNrd3!>EltHPkji?K!96LRVsU0T6T0XR-FMDQFwS*phF z`-M`WWHRqBtp={313jzV=YhN_ecwUY_}RHgol)@MsrKB$J)oQS@si~g3cSOWce2Q! z>>{Qm4y$|gCl}?_4!=-D$)dxBoCAiU)FDHNt-f%$A>?%PR~{{3If@mu#Wa$=?Ram+g9I&sn$#o#Zin;Od@4 zza(bil-GtWW51_XxAiZLh(h=c=6^}G$p`|7p_J3_Zk?0|p(*B{UQ=_9IB zU&QNBDQYsBR3dU4iZVL?@=cn7L^5t=JWkb#>I>fX?1Y7wzkw!9x~~f1HD0S zi#~kd4)NE4j;ReNqYDG;Tja5Jd@j@fY@8w3FpZVnrY$cL&jy%*CsrRgyLTEzc5Tv= zA8svvDXrM^(XwqvHqcu*S&xNEH52aj`7dPU91IyjB)+CD3KbE(Q(&PfR4O2ER|_!Q zCyek)crEedTyVtfG;zm^>p+dY4AaTrN9FkrI4Ol#?!*ura6L=~u#Vu$t8zDgEhd&M z{qAcXpo_S;UvxMW5*U1KZnBMUHddq<`YargTK!{`>?JZ)OeS$#tG>52YV9{|_q4XM zs5ZCK2#QhDXt(R?^Mk4eA?c>UC8UCIXoa&cv;V%(|{nUARFu z(@sVtR=9$^!6Il#bp;$$SIZ|g363cx_%s{e6Ke~=}IeH(vgrW0>h>< zVJ>ZK=Oshv7bhW6anK9`v0CKa`0#VcO#JXa1y>uIx9UYd+o;359uOxy2IQxfPB;Gv zXQM4*v%*c18oxEM;XWWML`VVbR=yFaw(&Zma z3z{Y7aF>B+ylkA$zkg%SJ3N+W)T_LA@!X1iS6vy?#)-r0ud>TF6$!I(trl$JX-5qA z#~SQNx1rrCU8l#7K3*?J#LJr$*y7BY5H;|8v+YB`62D7({ zZl2WG*-oWwY6jffz9PPfxE2odh+wQFL@&%9MZj43r=2kzJ2d|&w&-d9V(xT1v0?m} z{RdOYy5Migkhcq=6m7I9Erd5K`Qq6Qj~*g#6G<~UvX0>gP*@ifzEGcY3}Y{hY@05X z94rYZ4X)?3=&eT94&DhkP6xw;^XAyIGNAmI@9A$^gSY!{+FRqp6P6Ye&)VM;qfK;DnDAiN*N?Hy;bhdZn<(v7 zk{*0OIwFM-p~_VO$u*GsE_lScb@0WJ2o&$Fw`U&ORnG0=R&#RI1oczO*sOpD-BLX7 zdEP^q-JVpLR+fJ-D!p$JmOJZ$30%eA{`xoe_aa(Hm_(-ga8r{}AwTXZlpD9;FSH<= zw#BJX2_;%b%eYeRz&4(tH59$z>Xn{hFYzDkmEB#jQ^(iq=4mCx~c- znxHc97YnOp0k=KNb2R+$ZU-gK{C?cwa{2keNti3gJmM~IrnTERZamUDYV7hb9FVBf zQuZ{;?c0|m$X?)s@)whtPrk%)T~vs%d8HYi2Ura!HtJ*JrSvf~#La9@;rQZ+QOo zJ{ot!>d?3UNpv0RL^Jqwm7yJsDkhzmf& zV&-bsF;#}tP`~$_3)XXSFEH~0Io_z1=$@P%_?xW~4A08NM+a!DRQWo)frVbvu zgOi!wshI6m6nMU4iVAYlYrn&@%J*gtY0}r>DdoDYb?`U?Y|Fr{)!A-va38nNm9lyw&r2;wk3PrZw4orVe z;m(!(Aul&0WCH5!;12+lvj5fscx=MpS(Z!Z_3B42+5)S?*aiE?=#n{K@~w0nR!s?> zh@KHB;oGl(4m^E{d;X;eUrO zP+J%X2ozcNa~Yn?*( zLVSE5i|)5!Zcx=%*`mJoH*^`z2A_MfT|#2oZ=D(JW6jQXHP!cw_vVsv4*K?^ZIdKV zat(juE#{bjgcKks791rLrYKzp9Q?`oHybKT{oBOVnX#7BM!tZ?VPqk~$~tYt874BK zyjv7D6VEWl2Xw@d={H?rq;>irS>~jC@3oJrQS<$l571MRgdqfNwD4~8Re+dX!-oyp z6Ee)fcJQ$LC0~Nk%466JAxDZfZRf)bjAP^ygQ;tRJr=^JuXfI+NUw$0SkC^DipRe$ zCxjO=z?)=I(q!uA2+0+xytRWxqFY@fLfC691_tHZ(4on&pCUA4UnsyR7GyhbfcF?? z=SD`c?Lia8uyE0M#ie|8n2KEixyT{!mpn}2+~bdgPKFX3cy#^X9LePe;#j!MA^=bf zpNbYgApB6I7;|CrKy_0%dJ+r9Z;KtpU( literal 0 HcmV?d00001 diff --git a/src/cfclient/ui/widgets/geo_estimator_resources/bslh_3.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_3.png new file mode 100644 index 0000000000000000000000000000000000000000..e5f385e4e31dff99e52ba15c2aa5fc9b5aca94cb GIT binary patch literal 8838 zcmd6NS3pzS*6vIIiPBYy6hYLDfD{#k2m;Cm6htLbrGtus)JP44pa>#Lbc2G@1Vp5R zQbJ2YjiLq%J(NI%&_k~Y{L6FBSYswJ(7C>00I{; z{EY$tjspN@fY=4SIsWb2pMl+e7wpgg@a_HcgZ&)DvH_3-7ymwI8Im?Tc;)sD8=~wy z`u-ucdGhR`)FbgPXYHcdjgsR>qmHzk4B2~Ou`EWN%dWD1;qGGDuD!R!U|*iK@^csc zRk+Z$D8Kuao$f0G#8+Hb^o^yqa!ELzELDd|?joe>BDkA&=P`pZJ~qQBtBY2sL5!ZP zu;<02likHrg}xjrl9XtsxEtVt*az^Mz`&l{0N~mXkoZqU01QYT25_KZ47kDVCjbGy zOTdJ|n-g$3VBr4$-Na~~0ICU91YG;aO!44Vf&q1fbn)|_=PdZYB^Rq>O2gET#^4~m za{lw~f78Y_svpshgu9sjcNH7{uWP_CFfMh?|7xi=Co*JW3)p`*Y#9#PBt`PS8r)sY z1v;ts|L60mr^B3`sG<6~6;i|V$8zsoHw{&fac3?Dlnnl38ng$NvL^y0KSP&fv5w>} z_mhiRFHwTkb8qaVV8Xb35g`5v$$Fbay=JQBf+WYos>62Eu zxB-D1=2wnSt{?nte((q&f8Zv!dg4i2d_GnfSi!$nQN3!KuY>Jum0R28v!R{49P#uG z40qxOB8;c}@CAVf)o#>bJMO;$?uRhYI?^=iGb4WdNJ2`w0f6l0XBa@OG+mbq63Is+ zI!0%IJT8o{*`S~Rw+o7!EYObZ@%}v|wJs2Nb0Mppt_?vR5|D7tf`?tOCZ%Vl$U%#e z{UIMnku1b%9a9Q-lx=j*O(HtQ;uJzXVE@{ZrGafGJ{Wxnta zlG;p#!*QD2b9WA&6^x1ebBun?^2=u(=PgW;B5)Wk^@hkNfzL?Uw@C?J?spCe1HxrE z{f|t5;4EeAwVMk#h{NV%#?r31QJ$UsGcE|Hi+w@{>qO5~V^sm}+pk@>bUNwP`wW}z zzJlQD4ksreaQ1RBSTBU`qt-(JNVy|I;ybIm% zMC3|&kZlaFe@!_S*hfmOW@f$tmH*=b5(owylzcu=!%UV5c zOJCHdp#?%>7YGgeO?0)`xx>0tA-6UB7J6>PaYP6gcoilc_g*mK6)Mb5bUsYQlUt^_ zo_C>YpK|q;P1=T$oI_YV$#(rCZL2emB7+MQ0VYSrA04%NGp(rxj4#mep_t50PiMuO zW!_f?M?0i1gQ|x>TfP9Nl_LunC$uLt-SsTexm@mYlb>7g)MGWlyCkAaR;lX5y7u|Z z7X9>WyiFUrI zKh;I#pxR43Wx=&ug7gcmZqH0|_rD;E3Kad;tQNw(dk+$N@eH3?2}!GH#nZDHN8`uwkI5DnDh7zP%UAxt=S2Zs|Q<Efy@p$Uu}V zqMX~Q)H7?%iu6rcE4)5Atuj@XEe=%WK*BJmwze`?N4hYy&UI*N4Ao9-2!**IV{nwx zSEe@U!{W_#ieYQBh0sD%mVm+vZ|6HV#U`dLSya@A?gcAzT)09Dm^A5P%$wd~#a0w}{moB4Jc zlFRRJ9P1}^K9tcJUxkrx?Zyo}ilTcMsR4(5uxDPH9|?x@TsSEgLAf6J{u*h@XoAU! zPK-v(ya$v7-Wl^edm2A7^y@Zr@xwzI;P;W+;B>)h|I3OpZF&uP)YD)<7xwJwdqbjP zZh!Ypg|LnMO&Iy5I`6Z}`1u}9pP_x*z@-k7*ScXo*hvbhQwPc-EP-8WevJNql9*T4 zA}?SZf=uQ#lZp8?bsuliBn$59qe0$?ajGISt*(oR*>@%nsN2EiZ2m#(J@Ot5x;wcG zFyuhM9V8RNorz+uFCr4C9~=h>&H}DILms8o6O36@`{}Re1|GHV#N&wlZF# zF24&p4|AE~hkg8zw&Z$zvp0AZxP)_)7icw9jPTO7o;phiv?(XMh5btD+PKqGbIpjl zp&Nt6{}pG)#zeffnvWWKMZe6tr6VOBPL+}mHFhX zWw^lMTcY!XSqH{xzsk*EOVgS-QHqx7iqM}VXm^aszCm-u0)OF*0Y z5+<8D#0?k!aSc(e{K(COmoPXHUhY{?ug}bhKZxYhvDxv-2ZmDcS^Xy1X(dG zApgA^$FSm}_npD(^%Dm^FnGbGRG{sYY6}bB@4IXZ;N(sOj#2rMdJtDW%zA)ozxaT; z7rb^8`4m!I43{v^6jEUW-02A}ZPfIPxp)_;0oCpl4^6qA1J^!}WcI%s8#$BUup$Kd z-395|UEixp{`wYMK1RE4v!4sl%y|6LR=dl=TN}=)Wem)|5vH!Ggk#+EGUqe!T-Q;_ zE%Mep0m?X#huv8#;B1Cw?(uEL5&SVc)6N@}73`ND0kQ|E2c6UDkn zlI&HLZQHNX27mDHcO!rr*x1HVK3MsJ1iq`I0mXx;ky|6LoB*wE4e52X9vj%vHZvfb<=Um5kv<4Bo^2E3Y2)}ZgA{{=xJ zV0zEej%V&vNGi*lo7gZ!%fa^6E{t43 z@$XOFj?O6Y`MQ;i+h{C&=HvnqRU z&10($O!OdrJbc4-rm?=V>oR!PAv}M$Cm_#ytUHi3Dv>bC9CCbj z;v^_bhcN3J+`17NxX`FD{l2|R{Xp@JO~Zy*C;~r8-b2?ek_1AGSyMOB1q=R_xk*P} z@C!A{0!QFf)oD@?$Z} zM?l6EU5K9k|Fx5ICBz@5Aj#<(2LD8Z1-fxZE^mYnfNR^LloZH8y53(wM)N&~3+v&1Cy_RSsl-XJ7XYx-X7t7_*^ zy36Y#_ztT`d^H`BXfLWRWRbEezm#cT`N-sGD}68|rlJb6SUQTiHlwrl_I@_> z11mLWJ1HNN7UR>F3Kv`>wzqY2m$N?d{(`D+F?)a5r@rh~sS}yA8bcarqJiawVJSju zHPgxtP1QG541~?wuLw<<%jomRd6_m?&!`cs*(XcRI<@?XsP@O<_A=S&I?IG2ghs3g zyPIe-pVU$n>`bjO6EaiOF?9ZfBqgCX15GF6QSF6>)lsRP7}q8L!Db9~i9mc7s*t?;iSYW36)S`ywCtGoS)6T}vgLGAcqt(9>|W^ff9OTMnKnL)MDml(rm@zcyo}I8`$T6l-zH(L3wu{Krjh)=pnw)XmL}WcAk#Se zx&=vUXz|6Ii5830p7#6c(97V`9+lkWbbLRHPZir zSJh^(HKauSc?*L*I}u+~AarYdTfi=K$IoTpI2@Kz(tNbZ`TB=GUG+mj(Bqr;+g*EB z8(v{-nDC)1iYrV`SCEBH0q#45E@KCcHHLTaEu)Esi7&wcj>>3v*Cwaq#uJ6&Q>Vo5 zx?DIuBQdpNlYNM7Wq%2F_%9HHz{c{$Tynx!2cxNLqw=-S!LU-P%YFC)7lB)u%rug-sU3~|3Xx6u5%$*iwnQFg z=MHrvv6s(QhZoS#e=CJ2%m;TDZbnFpR>q$;)qnJCbr~~0TUaw%eov}0_}sFqCG1M< zeMf1Eg>T9xc4qw$cS)QT`I0$5jO5x0`n#8v=m-ae205F$QQ70KZN$@;g5~{kdu^6X zVSaLKs2S_L*r%`rcMSK=>v>DSvifFD-vl6{xH<|s#Z!`JFRzlL)mY0HMJac2cccUm z=&M(Z_VrXV7(KVv*15ri*J)sEi_y}nuQOer{u?oXpG|VAzdQdL`4X=ddVB>l*A~li zI`IS!_H{4HKPzck4RP{|--y~;7|FT4v-s8Tu8bRZ8 zm$qd;_fe%^HAqv2J5?jb?u~2vd@H&-SIBoIMk)D9^X(!R9uQ)j2=n_(LO>wbsRA%s zk%onX?ITvqp|cGfa@kzt{RrJmodiVAAEki-*L7!}wDF4YnzQpY#H{w~;zQN~j*hMK z+vFZGtNC#m=1n{gxNT4fo1fIJL!vju4(xgAXloznMVPaV30rdv%5;tZFkH=Re%vX; zVneGBSH#$lvII8=JM02^{DO}gcGVlX1_WUUm$gX#hqs_&iP60%?5FknOvUxp_gZ+L zPp1q&mLGe|=U3=cx?ItojLtawGCl^!tb8)L!w(TdA(y|v%IsAz9xhg_(CE^>J+#uW z;a|SZixv3cf-NsJ9SXC|ED0heQg?6R=6nBfh~_I zM41XiskOFE)+8;i)Lt_g_(AmFz zE8*tYAVj^K?!5YI#!x0=c_?1!j~ghFzj0USn=5jBqCcmb&A@-IZ0X=#e>>+T5yhMf zMtt)Jb;HhbIoGw9&kuwatHJf20>PYQwCX~3P}igqIa*_I*Sn;EPAM$M<+A%&7OOC8 zM!zkVB*j`&lOl|6)oz44u5Ya+^G9|?LD}l-uW(s%%5={|@>&4ZLwxp)MYA<`>u~1A zPph@TbVsmQo!s~Nun&5OFEZFnF^`WZ0B+kt=u3b9rptA(IxG__VgGSO`Htq@hM_Hj zVCD8&N@}w8fe+~$9L}LJC<}0&4kfc%{nG-Mw%tmGeWIz`3-z*A=G`sA>et*65*Og~ zj{#KP^6cOrm*o`29!Pg*bw?RVnZ=T`SWmzEfB5bH7E#EnIx(#PPC{hhGj6i0g)2>V zqIpejyg!{vEw8(Ta9BPoa$(IwDKR3mrQCL*?hst=8brHZ1P-NE=jjDeC&EPa2EA`w z7BcAGN!f$D^cWy4)$TT)xMa*f{0Zpb6ejPECJQk*!C@@@ zdip9LzbxOg4nIXJP4Af7(ediji~KSt@iVR=&i7k**vP`&^NM4IY3O1&F%i8jpS%1I z#9uSy0reBIN3JRp73M|v!e2AZQ_M~5%N;E|9qI%zZIZh9QCp`pbWvs}LowGOShRc> zF*7d-`wX>dB=E3@y#>Q53j@PtSi!leRUR!&xp?oCm{x3HX7iW1`4ju$IlFHPU|FgQ zhdnPuC^H>MOnU9A!|`0Ud_g3Xtce&I(moeEsACEo`G{&yoWAB*FNS{MSlaEbz|@T~ z{{CfZ6HpT>%wL1`)cEFwOY~at7sREKpW-loEqMRzM9g+5b%XRSY5Q{caJzYihoOt< z!Cs)Jzg@Tbxdy50Qj~F2;x%aFe&^4{OA}zRF6vsSzmbXf*O82sF>@m1U zj#Rhg@KjwE2&;5Im!88mt&jN9Tya{bA)AtiAGn|4cZ1gO~!e6E<;=wXaU~xEQlkZj965j23f8xB2iE0(S%(%E( zbUltFo_sxdVKxn7vv5j>8Ji=#OC_r*d=if zJ!0kW@-*Y#sqG3$>{JSxxwpr5! z@idXbJ6M(b12^xLzMXr4+8(dhi(1i^f3K@>-enE#gwOg~Sy=iOD#$XBCcGk#hG;FU z`oB!dMb?`r>KV#gCqy>=TGn0}xA0Xw7;51}T4&ZRl_P1EGq& z5kl;l{(O-wD?!_#HO@=c?dKdZR?>?XFLG{S;=ccuCL^vmx`g`7IfZlX%5~ODvOL7Y z@1ch>rez4xsv3GTSK2e|WY|+*@srjZ!Efhuo{E0M+%X(&!qm5L&Q91|sqSU9AR~P) zXl<(>q`qP;{&wi04g7u0hj4*fLo8!62%p#7dA3e<{2IDnwdv*)c>{gjDfw);%R5Gm;YDn=9YD8^`V@fAH zNX$V*T~^ zDN;xepEq^mUYruW>E#)k-1vfkz*vwytebKxxay}?o9n55;TH|07pP4Pgqdr3cUpW5 zJ1lD$HO{mvocN@lq#4a@*~Cq^^#@Ld;c9IzyHz@1^^d0uJiWbFieiRP?bwR2(2}sz zCpGDV83Ok#WQO9ep0zyCXGU^7z)$+C5OSy0K_J$Nkf5-M&FX{luoa(*dFUhSiKa71H88b8ah(O6Dn;EB9*=H$29@ zs#weG`a;zjyk|4ELQ};kv|&n!MxDUlb?FTucf|#BDCe^}a>BA@C@H4tA9bpyBOJpsi$;fXihuREo zWb3@7eN?`6_|Z%&D?Kon`j$CoTXHs}ivgAJ^X!09O{%T5vC;b79|J@Qr!!A&yqjcm zgnJV8FDP-TQ21bm&`IHs4zcFLUY_S#u;8G^rbqCMe7kMlH{ z>*Is7BPX>sd*TQQo%o&L@Tt;o)@786EAu>E?Rdsc=}xL&)Jrax$lg`XmpQLqzJKm? zjSLTUOz>8l2`Eyze@MN$qRw*)b6}YDjju1xbH+Q;r8j6Hd+}{u$x2M*wbDRKEopX8 z#e>Nb!w&QKK4UJ}yc3&Lp|5p3s}H1P2>B}WmcFXFpDg%%;UksdHXv|HEkY#q6)o|W z^9DMfl2f=H6nR{>6{**7=~L|VessJLCTEw&0xNGR*`q|tmonRt+$ZQSwG5{>Be2sB zX}pO&VPGca?^;n(he`o~j@iP%okO$zEiQA#|VWkQqFcWw2d#u?^4o2<%a2wo)fA*m255#83?Zn{Iko31qCz+>6 z^gj&5WgXhF67RvZ?(N%@{Au2bi$Geq^z~ZyVvjdZ(9pO18mepTd&JzT*x|P&C-1B? z#($?d$2o7CMmNg7L}3R;YyPnB288=*OKMQX@d8C0(>-|$$%M+9silp&+@#Fh;qW;H zh~$T6cRb+x^>ga7oZn3(Q`U%QzK;5ryN&6_@m$Mc;;%a8m0|rQded?}{lvqrR$Vgm z5tL|Uy3ww9LtgKaBIgKH{+Z_MQJnVz*Vp{8=EtLWhFZ?`#pby>Geu|ZV>@y^le&tVA>3?&u>U!}lW}5!YSqh0M@&Fp5dQtf= zB^t>AUD5b+#K#-BKQs=|eF1S8Yy8Hmm_Aj;ModAX|C{2on-djML*pS|Ps3KKb1O*FKAXKSJ z=*7?p;pTVW_m8+A_QUQuXU^=NIWy0kc{b6+NRJxA4gmmw`u;s_GXMZb0RTvy;u`UZ z=+MxA4cC3|JwyNiCH;RF=-2NEJOJ2%$fW=lR?J+ z*=2?`>{X`dN5{QKhOQr3T^d@e2Yc&i12QWKI)R7g#@!ekvvHW(t3w;uk@>@{5nL#N z?tpeH_bySvZv*xFABhToqXa>Ts@)}32Y{$SC;$S0J6ZrKv4QIUSOe}>#L_z0a|g|O zceLnBkJ^Ord0R7LH;G3S0RGk>URGR7ZNTKSImd@k6+7^-I@g8!jz6>2A$AlxZ$n(3 zPq2PWa+*%8nu`2#E?;ogO?qp}gXqf0wNJ&pKHTk7!P~e%?XCD-~#@aob z7@YEn$8Df6XBr}oP;ImJWbt^vogh0f+)lT=r_i(h6|OXM@=SI83_GYGXQDG|bxn@Y zKH19#0E)3d=54Z`te65hzu{E(7hwhze&o23BHA&?!~#PMFHmSe5yjSsF=E2&qD~Dn zSFKd8a?i0CR$O*>8*L1NnuP=yWd}Rz;x}QQJ8&FcfPJ%UrFHK11$|F3l$4yEJ)i5~ z764Q-f+QIroCVdXKcfi#2?t71^4M~wa4{azfui=NW(@h~15g13P%pD4n@4OuWMTGu zF#W4Wsj1DSj!Upean@X(mvdic;;%-E;$t&B_(%YC|0y=GqgrL&H_dc4 z%eR)5seqgPt_0_~H2o$BNGA>^Z~8i|*%1OIglIQ?ROHcyJ|I1k_OfdOB7;i&85#Zjq;o^sfXJiBC=Q-55t%iYOo|mJSH*ucz`KHk`N_ zQpo!ak0#)juar*&L4YqS*wOf`-O&_!h{3?GGEJD2ji#_ySs7wZRBAQ=nvWt%g@x>M zG;`wVWbsH%vjI{=IQH@x9a#hbwA=umXlnIQVsmLJ>fOJMlJ^OssLsP5>$R&H1Hhjs zAd?S9h8NXe{RXYTI5s|*B{HxE3l84#Po}tWL~{l$Gb}F(~o>O z!gf{fUo=1|?~#F^ddWsfMUxLsFS|4)4|}{fD_K3dS6~d2b=m>ogORIW3IoO5OWKO{ zU-)^KbD4YX!o-uY4-%LkWQ-XeBzY)*N&#N{V2oMLGJ568t}6>K3t{@LP;X5Aj2(4c zAopxoYzu!|fF7urg?Rsp?gPCk8kIBUKN69FIdCbVOy#erOK^+&X}3yluG52g+3E5) z$wP8=G^A)=2tZ!&-OO&}V;%~|^^NgdAVvMDKi{MWF3Fx0iGe`dks}Y9fMJ{UOLKSi z z!3Z5W7A8lz#oy{Grm5bX9PRGE4EN2%D;_2W0u*gI+qzf^bSE$ zazxQ*^LJouJDuj747#jc;Z1*imdYdaljF-iE@?^T&^9GQc;EbKZ5%J%oHhPqR|jnN z%37NH0QKE!!+r?q?U1ww$fAcV!tY>7!u1~-de5oj(Q{gW_8WYn%E_7cr#9r;NUnDD zi}EDVL+-57=iM*XgzVkz5&!nQV6tjr_@gGAR5^6>L98rayIvci)9Km64|!t?m3IURtMzLaua8+QfVV@yS1+u54OFoG1oAKdqsdcW4WWax`VKiUE zeoU&9Jzu0e=sDMsgtW<}!##I2v9i)}07a>59hb!#eme?#&P;fey(UF9H+ZWX9)t?c z;#(RnHU!CLlO$eipdh{X!U&Hzx^(8gxxX1x`Eb!EMcT)^YM{v&3qtlFW5VuG2Ab)R zL8tp#qc?gHrH}-N`*JH^C^~O!H_u(5+ECF>t)=!u1>@bjPA@`-=FuT^M6aaXXX6vt zp20RM?YU#P{?lEMlR7V6e+gyWSAM>rDUv&a@Ob%KIzB2|W!9UtcDc6sm^zPZ|F$}F zF<+lk9c6Jb)^VW~r{%ZW;Zpr`lPFPTmMRJoNg!=oBc2P{USV|7iH3Jn<5P(P z10iazi9X-~|6vAWQtvQ(wd&DCa(AoYVt1?TfAeM65v2CWraj9Qe55tgw3D?kD%8=- zVVOs-B4MoHx>t%#v_~(_-cH1;*nBXaz}5#2J&hLD4&Pr#w#UYQO9q%6r2&o{JzUw$N)0z6(()7375V4s zK>IYsl*eY#-dG(QiCM|OTZ5CCTL`uv-z=J1_e|clwo3u}+T;9{1g^>fKFLnz{*=1B zcpxaIH>q@^N<#I>=U-ZDid33R1(M~tZt*d-C!7=LsM^4>X+;ivz=$7prlRnlyqrA; zJNN+Zs+SQ4NT)v{WEyLcN4dh^_BfmuemYT#EY;OCTcKP`Z>eb(V27%D71nLqyuhG+ z3izK7bqN3RUANxccs7lOaM%fGV#}>4xQOZuOim%UINS~znL0RE^btSrZnNi3l$9j^ zmHhN!peM~07;}BNN5d5}OC8F0oX{M=etzM8A>7O3n<=%UW>mrVD`z1wd``)_#1(N) z!kqwFik3ccnfA-dN^-nFRRX9GDXU4JUR&cOdHAK0D-!=(JCyvyL&y%t8Qvk29n13q z2#OLv%)3F+f)xju1eOx8$8}O`BzEY`8Y<5FxT|=-2W?72YTPI8j~mJ{%2NHE{hog$ zNzo*yPS$p9HU*W&(5DMkAW3);v*qb?GqBMFCVxJ>ENQdcI9aoa_e+C5p4eMOb7^z- zi*t-4;$_!<C<#zKcGn=3LM$(6wF=;WpYn{dpI=~tg}?+aWx z_KfMckbB*<$t>-=45i5{b~t)MkCO9p$V0;NS^}45CG(yhFt1UaWO-X=;}(T%#n2MV z&W*dm+Zy38v)c;Ey+v22v7O)`&6A&O}VQAg_=Ccf*G-zWxYNYP@&?W8B>o$Aan zOSW2XY}Z8!u>mOcN$dS_&pX>hKn|Wa)p9;pbTZzQn~8_sc4YDm_tRp7=iQ3TGwGQ$ zbwFkNJJBo!5Rf?eNjWWPJ>TNJuOoE1TPtb&XB{q!iW(xlu8;>2K0^}bv`SeWVtHtfPL$}@eH&Z_%NYOjG;p>Wf z)rubWT=9^C(;_eN!vxdQe|_sC(kY5#D=%jgFyUoVO}r4Bfir;$n5@syGEC#nQ)4}- zww0Lw-Q$Z@@VOS~~ zy>3o%*Nh%)IN>Wor5gOnLMd1}T8HDejk8BOxf7WCowm5quJowU~Q zdml_3Dg_}eo_5OKyU%m#ZpJZvRpc(|*cl#f9@hzA1m6|;J|9G^(!$~LsbvMXLpl@M?Vf+_ z@8)2A*O@5$Dn2O~wuF=)KfJppnZXadFI>et`GL80Iv&KC1i|Lh zSL%G!VVqi(96CMu8&}~dr)20U#tyO{Qn#kKP|5<+=6er)VR^e!)Q{kVErFD^F<8=p zeC5E78g4S`fO2Zg@IxnuS?I4ADE-NS!)m_jnxaQs=gZ!|u#Cm|F*1)z2Gd5taaKO4 z9sQ1r6GhDAC_-`Euh6209R7J*9T24ux${ubzJPyi=cyT2{8Vb3+D4~m%2`|5JEAD)j5s@z7TpWcu>A*m`UC zxi1a4v8gtCx4ETu5{Wirww&2i44mT zYz}4D-bpF<@M^wE<;@Zw0W&omV*~jr5r^D}G_%;J>@$c>5iGcX29@8RQWw-o*Whup z7dce0^Pu6gZ%$H_DAc2fLUV5lT*a_IknnqCTV-X5`K(82>)paIv8howUR-`cI0|vt z`&$*Ch?Q23bjsS7+z*~m0TW~LlDg@BXt7hN(caRSG&PH%^i)!^;G5;ezA>76Md_0a zWS{2~609I*_s100vvIfpQuG|=cK5Vtz0fIJ?RMVtN9~4ln_`E+n-Zs$#P}CJYp?#= zPGW+0?|k+;iMO~4;0Ks}P4%EKe9V+|R_^_;9cN!93GF5Rq;Jajhy&EHNQ8#BSuVN! z4}aq;Ejr+*-QBqK`d*Vg>X^l4KZd^&**piTnww3#2?! zB@Vx@u|ov3`GZX3Ru5{I9@BhHf(?-fV8?M6XAUg zZ|J+AJ>_@tiRDdDmj}-0#LfN}V*y-<%r=VESeE4(7)nP9Z@&Q_{*~$a-q09c1{=#O z4<7KL2Tq8Jj&k5Zuw=q0zB@}czQFBgOHQ5=MMN{MI(qT321UrtBbDdIAM{KC@2N;C zzFT?A40gO+J3%73Y9O8ow2`Z%Ip4vpH|IIe#1$Pt6AgGK>{%ZaXf+7F>JxqFsg)dW zm|3}#t2B=9e#;WTcmnP}@cW!=PNtnE`DI$=;{%rU+&G{O3D&~c0A+kGc}B}M;cM%^rOE^ZJ1>aqAvR{S?33>0i!L@S#( zm{_fC%{lYcc|2w)i#d0B6g#A#+uAagyqUF@Mkfp}FBrHP@>YTg?{PVE%IBD^VHJDA zc&@3UeR(FB6Tok$!g5YS8!_C?bGCtAz8MQyqCr_JKUy&Bz)N;E2UIOBEiRci`xeb+ zur8Rdv#o(z21hPs`#X(~_JS;c6O+Bh=CyP>S@vSNBjUUHP3w=S3P(*QEtr#p`18Rw zZ)Z>XYf!Ez!n$aGbK@WN&~O@=5#xLj%lVrmoy(>|XLhl5dGm*hY44$%sQxq3DXq_rigZ3qh5*~Vsbh8)1IuD>e)}vBm0)N&E$h2hdP2ZD~ zYTp<<;iSH7tlZWnBARt^AEUs7%{W*myKWY0C2&ze?g?Au)rPC`QaL1%{lnDL(5B2m zhr0_U_k0z>OVOH%GjM~q-x;b*ar5vF(@YwKTf7&|L9vX%?!xOzDNqAUWawfwB#fLC zy?QElc~d&~O9XP1r3K6OF^L6*x=RbaKeh&N4BPFPTv_!ozUklxxHWu&uGR|f`Ze3# zUsp2UdOup3^+x4E5ZAuF=A-flpjN$}9n%=`Th zdvx<7R@U#Mn~Qv)hG6#K95>R$eJc=n>AG~ZN*wyBguTaTr^z}csGg;WHua$-RZ868 z?55rn>da`$b~# zd)n#kT7JW!G#-sJ+ z#tVpfOyQl&H^Rj~QO8fyP!Udb3K|`Jr0Dmz8JPtw_Oc!5V(g&3H~473L#j8n*x|ha z5Ld*I9YGzD;n_~1cQs`V`uLW~0l94Qp>zXrQ`)6_OIe+4yFH1|92GId4}jN6ndhX9 z)2AMe;|?{K(wMFM*4AKr{$;nXZ=0EGA_9ydUXY0KhFXt2InL?SJaxeDYgr7)qr8@5 zY77HUjhCdZqUj5BAsfTc5onl?zo-TMh_B~_duk;lrmn$;DI$0H0B|VLB%Ed0X*5$A zTP(8&WS)9P8c8}?A1NlWvk^RywFmKc_FR+l;EW3@J+aTkCjlS}I7J0}k z8s?PzppX;{~qOvD52Y&2h_MVD4JF_W$G}52IkDW6rvWH)DOHu;PloGew+1us#5JOv<*jf6#JB7nI9IzK zoAsZ0K=rffgsq3$8?RqZYl%c+hud$Bz*=v|A)idu)|y#{=6&Gy5vCe$=BEsKsXGdM zxuFBRSo}TjOy<_=*0Y?5`smr!FX$ED?S=Ookl_BcD-uCB7RNj7L)6-iu0_1hUA%hX zE=$EhvO;gt(;dgg(Usi$kiKdZB;Ls*#?hFLxGp-=&7g{zhK%PO#U2h6!4WS4E7jBI zJ#8+e(k$q5-PNAhJF92_q@zrOpw66jl%bo6bn70fbapCv>LYlYx5fx_^7CElmk$tD z^({ZC;C_#-jtv|)+OC9eED=>_P@BJVNu6T3CE?fi{>v-bAEqGEQL>24jhKQjdA|?e zjYps>tuyDQupW!5aM3$vUxPQQqkpSDda6Yx>4eHo33$rDXsf5?De>w1#hK-NR}tlO zZ;fJhN%LrEm_A~$V=P$K;^LYD?Bdu|M)EL^^CLt>j1>JvJ5}lOHhoG8s=a)^NTn5JI>k&hE9x+ zbJxzN06X4bTzR6%SL^@0-errs`6UJH%WKVLohB~2XlV18r8UCvjvlfXQC)uzI$w12 z&@SG|2vvEp(cCh+mEyEa3I!Mn&y>y6XI=d>W=e|w?g!oD)wW|L)D|#{#$f}CyapP1 z>c&(3r;|?%^RT_$Z8K_W1HTSq%jy_JwKwI#jtM%5;1vC>TB=q_E7^@ zC}>KLWl1oCLs-1+DEYWB+<>VL8qmlZk~aL9?=Wvexux`%6}8I1Gal{y@s~G%fOoso zSa$X;i^tk5JMvI_q~!F#TOm+`DvH2{A{$DISLzF$*4|rQ7$m_M`YCrI$5O7XAIZ>; zDDgGqP(*RL)aJ#XMcmFU^m{|i@&*B8)?|G^ATS93#)Su652GHZ=H_cCA&Y@78d|8{7W4bq!u@WDT8_|*tjcDKeQ8L-fMS|zcTI*nMGY+dA8Y#vn{DcWO0X1`r(ZOfMGyw;>$36q@@OK_BL3ni*uC_JM$ zAdn3qar$;@%M@!wu9+(yQfs_%pZpIJI!&}Tng&e#^&jT>lpMy|X(Dl3obchphZEe7 zhENW?#&PIfUZCXxqA2X|-h|T|`?*HNgJLK|A<>VM`Voi}4y?_or3RVJY5`AQ3*X4G znC2EnZ@C@uAQBXBW-rdR=f4NC2i(-N6VcA!Ug|GK+&!Lp9x0w(Dr_;0*+gneQhB)#*LDTh zUyundduJ8>5tAPAL1BIBf@>`-qjx{a?|+N<&5D~!k-F+*#r;I}a74bX^jzWfI8ME0 z0(u(V`vse+$^-yR*Z$)IL~S~>>U`e6>pVCxX7c>mToqLGBfAmCJDdziI@k}Ts<`GtaRpbv1Wp%Yg z$^E%FRbs(_h=dtnhND_z%6(>Iy5~-1v&kOW&BsLqu?6+W+E;EX4XnBJy~VHC8Plqs zW`=4kkwg)A0a+i;p)5@>%Jz&2Sy}6Z8tv!J&$%Po8L%=Ml(MDy%9f>N-nobpIOGI1 zXe{>*N3=|r?VlHJUIQxU>-XQudVJZJ001`*P{TuhVMPEa^ddz+A-ZuueJhh>`c>s6 z<=ZU~P{lUU9_dTj3+{j)QyYOm4GLPQNFYj^x(VsN2n9@)jujrfTa@nd$%!ICzpi^Z zttL-K$i|~taT)F1P>QG_Huri905JQwyJ3&+*+wrn#pv)3v2D-tfqydw?2f9o{coAS zL4no{$SXBz-WmVn3aGCUvEnFXsiNyjH?$wVb>c;1Q`^O1`8?wThR_SYAYU%1X;w?V zqVmgYKDLSI_FO)ojPO-glOk_mbEUc}*U{?!s*r;tmZCrGu{(#YU%ZrB^H=H4yTHBJ zJ*fA>&chDSxd^ppOoUpPXKeLh47HTS%_h*J8OH^7hFY^$sEjYyR z)I&2KKWkUbJ%GPh2!ba~Q~P4Tq|cy05tz*+~MBAP(NNUb}NVA=q`z7v+=f{Wye z=3}mWE5AhVr{#*Jt-X+w%mQ+^l(TNyNQ;K(2ppI98Qm0-E|EZ$G2E_G-!sNNs&nvB zW$}A+{boO|Qu<_sHYa?Xl|j6CkxI)$eXgcCxz~t>Ww_n+kiB~k`(aaHjeL18n408L zo2-FN6nuYxkMGjn_SOUvrx!snJwi6oStW58NQ}SE=JWkqwj#e{1EmkQe4a$msYK15 ziBlq{_&s#V8bBC$5BdI}!hk)RtDLWK<+{*zb8;QisF*ZRtE|EI*fze<^#ApeKvZ$(xkiutGPBMErd1#ZF zFyf!w>S}B`vla1}c)#z(Nv2i+`{Mr|jwRLNR0{1L2|M?@^VTnMGQ>@l8kFAX78 zKO_QeJCnw{hFO&7V2t1e3~icL6ACfw7Q|#ByjMul)$VUf1rDc%zcN1>5sIs2oDFC< ziJA`-)j|$K33%+~%a^WNS&9p0Zf`Px&uZD6Dq((^C~lB;O0dOYq|JpUK1kD@&~mNk&Fb{Jye37M z!)In7s_voN;h@9Eh4I76XMQPa6Zhuk*vUG^kC^8E+JaI-X$_4YPFAB&^9ex_2}?4m zFCXva_P$wdGouJY^*jJQhN#sZ*~$F7OtdIUfu}xG@nq zJ)btR{U-eHm)LGwn8%8e6t2x7ljdqYvf^E=km+rNJqe2iP4JN!RgkJO+*87T-ucHG(V;%5(?UyF{_(CNh$Sx@54Xml_GNa{Ybh{~ zUV5c}-ap9rY99ef_`2`P$jV|P&>z3*%*_h`Lg}6W)#s`?2l^zp!ZV-h<{}zJJBHl5OkikwMg#mO`9hT%n$Wt-^FrUJtj{=V;u?E7H*9?t;)>iHlO zJ_*uN8_L^FU*XfPa;FIAL5Izgg81@x5RW3g#oJz1D@uK}|o)rpJ`3x^Y0?7UTqRHPaN$w%7X5yaU=TQ3~%B=fJP zlze$^xP8$~?ocilfl+An!oG2zFn`sbMf~FLXc=Zy?_y?~V=zZWp&-xhMPUd$dhzRO zX{|>Sa^CfP^-8)QFLp`T5$o%md%1<{;HLZ$DXSWUn<%8MX*QC3n?hfB_wbW+r)d0kkM0kcXk-m4C;SX43QaXK4 zK#yEvJZ%D|+lnG{+@O(tFwzaKvB~F~o{vx;$G#*&Rl*VDV>UJUwjT-}uGEA0izqoW46b5tcvq zP0Md$`=F8`NIpkDIxY0jg_-xFFB?yG@s9;?f?5LZQv=< zn=_3`{&QRO-M_s&(rJ5}3IKdv+gqcnEGBN0Pm7-SS1r>LmrS1&o#aIs9j68WqfDe| z_IF5i=fOw;JP3#q=LIk~A<7UddyBT!U@-iKqZ7CN+xj)e;H3m zXb9XFq12+scSk&_y^LKSS^W3&q_%m{?c^Jg>r_PSCuFnllyXKsXGt!&Hqf8BXYIMS z)zIUda@MPGf{TlE>A>ghaufGc1s`Dfbk1WjTgo2TKNM9K!R&INWv z9yf<hdBnXt=BFY=Vcl|~^y z`wV@;;5>MeDy0S!+OAgHJ2<%pd?Rx1ilK62t4_;{c!`Yu%rdDbbbDVzpT5OvX%-5{ zIy(>M*t|S~58Uw#ad5=W8`m~FusVm4?ROXw7R63|RIUh73;!V^RvZF5KXmq8!hYQL zZ+Ag`RO~}6=LiPJ@MVzoE{s>{%YbUw#;VcmavxkNMYC}15uL+K4u|t_nmj&@iM0OZ zX|57c%r|m%?EF;rxmpe**?x3H3ya6rM@NSzr4Mp;PeS^_{coLSd7C&)xqY1qQH8&j z?h-d#bjY!L6Ap-0Ed-}o*v(bK0|rb%Bvve50+&XN?h&Rg!JO z7MuyDW&i9CKa>b)7B(OI=h;Rs<%;B8RY0)i8=HUZHrkKWF39^c2Oxhw6X`{7C%Wa( zF7&_n{oJ!ZnjA1+l!MM80OdGD?D ztX(A&wnd^`FI2ztBqO|0Mb0IAk6qnrqIfjV8dwQotdo-GZ}}7ImG~faX}O3JuX>Wa zOkF2=`Q|-_Z!hYBb3$pojpyPrIc=x#98jrX?-c!&e7OBAx_|{0?~qD3uZE~q{JT1F zm`WON*gRZfmVy-QZKPrSTize^2@SW?uJ(*yVM)d={4aZ`Gu{1<7O+NgAUPaKZS#0s zkE@HOoZH)VMu$8vAvDMQk`Gs8ha=}_~H`!V-+&*i(J?6XKC&u$e z`?kZ#&PV5j17a`?<&itEC8-$o3`OO9^c-^k?FQ*H!Di`YpZ}V^(I|Y1z`b{8k653o z2NS+1oRHw2GwpwDbQf749C^#P6#tkFC~3EUGM8+ap7)UPOYbY6N<7k0cC?{F;ThdG zTf}Ky9>TRe%;_5shjYZ;^QK=6E#MiVTc6s>E-2p%?yl7_T$VSt7CNYoNH0VTGhgx9 zsuMB1aOW5Pym+&vbFV%q+S=-1x+EAo5r=G_WdbkP9~MWx=vUck`E zN4EBBXy_+>k;m);Gcvhn0Y$EhW=`uL3iN{he5iqGTv+4IdKWLCr+cYV4m1C%h+y35 z`xIsR5bY051183oYKlt+cALGw{8a&h$H7^Oa5q7wQIvyw__3zSFX!4zMB(m zAEVmlD}8>p$X^zMM5+-}i6;^rTYdlpiBIW{E^9`5evT7A@9150co((+2kl$(Dyg%o zhTl@Jyc`^9UMd{aiafD@Zss5Yn3j z3NosI+P+kA9#kU+z+^8anvLeGv{b1cpOiD#5T1$WtJeH#X8&GyXIz(aMms%sAhQjq z=#Y>PZ~x=mdmXFsb)eDTyZ9Z9qYr9SyZmLO%d{Cs4LdgrV8~967N6}n^Dubq;$mM% zb?uW2ifQLcqkjD7O7ERZ0zZuWs@;3t%zom}NKe68`)Ih`JlG&fl56c4!L8I=X`#@FvAdsM2eS$X6Z&t>hWs=4^4#!wq6)i5RUdq{brB*_ zF~9grA};h%?Ik(^zV;UMmm;c_>-maEzR4yeHABgAI$LaDRzGU8VgHfA+ijz9vR@L` zS!YE%TH>tPC1;isv2^Q6MGJ2w%2AA3h2@RPN!Vu8c(j6Vgb!q4Y8A6mw;pb*c)k0n zhIJ0l;#cR{;{!Y8@|UXh)s?Q^UWnts@BS3>wo5+LC#V&~hgF1xzU2>O{l_g_OE9WO zqWaEG_C#3M-^IR$XRCd0Or1BUjKaicrC~b$%|6}*@fq(!#73#u?~kSq?GgQCeR<=% zJpQ^##>Ifk4_JjBjE+Bc!0Up$YO!zKoP z+F>4V*!yAr{1b$QCnIdwf9AbEN*@{gqISq&9FI2DN38li^xD$FFf{cfnjN4W*r1Zv=tmL!3?%Fb|JkGvXo1dQg z*oO~RoUe{`oj|%S^lfC;wYhuyVGq?La2`t8y6hS!rcM&824ox64JY;BfH$d#*R;*C z@Hm9c#IJ9Ry4SgNRSf&uMH9Q<-_r7C32|stlk<(r4Fo@QqnSFY$~oUE{WNBW8|KnE z4$G5dSt|XlJ-EYr&aLucR{-*R*)^n$a_#wI#F#p{ea~G(PYSu1qJqDYoe`8u99^|( z4m^!qk65%p`&BUxzb^_&y51lFs>|QIRlg7xBolAEmW>vIOB%T5Zx7J#ahlEYva72W z=ZwRo?UpgA-pk5sCwx~s7T&%@T3j~ykiNqb>C7=`;=cI&#`)XDPx7u$IFJMK_^!YV zC-QJ_Yr}IB#3+r*`>Nhm6&UnwTPV3nft%5m+OK;9d%N;_mw}h(p^328aGAGWOHmAu zZ9r5ItqNUS#}gY_HXb2I!;*-=G!*-tndhQ7vA`W?E(2@bh<_%sQ4zmV{}5@)5r){e z>B6y2MxL2A%1#ta-wcjHqsnUcJ2BUXbx?4pvLN)JuR5?LZVNuGwNu`k0&Au{$smKh3RXfc*-m6~KHYZyC| zZ7gNRK9+~EkHOfNcb@mJc;D|2_i^0E=la~sd0ofxInV37Vjr3q@^OoC0|3Bx|DU^1 z0AN7_07#GX6mvzsxA(sQm-jz5egMF8=D!OxI1zpb0Fw0kcmFmI%3Pf=di}&IU3%Tm zSU*sI?0qfAin54>)k{~EmyqvHP4H{%P_JbC+a?EV3vourODK=bb>95Z`%0hO>)k!E zEj+Vp?O3HCnTQ`sB8r!5xy3Nj@AH+ypO51UFz{)})$#Vc4DE@vJdyr8fn4*V<`y)Sg!4svWxkRqJ|HyOqJNEtUo8CIx z;Fy)W!rpx0n$#rhpd5bP3IJ6e-&N|G|Jt)o-CYksP8mAbtl!VOJ{1T7JND4a0jdHu6pY(KND%B8={5gm45)x{ z@!5~k9Ep2M$H$0vYQboRgs%d~J8_C!{ZD*NRrvKVU?pib);SE>ss6=vrk5j4U$vsP zv5^3Upi=xAvi-tCqc*G6HJ=oKyql-UHeb1elocXN%iY^uqpqHoIRk4j<s<(% zRR03)%D`c1Az@A9iL+isv`XyYee?O$GUn6QK;E5GWOXm0^jjdxybhyGICgYK%nNP^5UvI83HaaJ_BkH4Gg*Q@JQTO1?OM0#0VDl{K`@j z9Y+xm*4egIeJ$JSXls>BFcAouTZ)`kEYkR!ZMWkgnWlQPb==0vt5?Cs0~mj}zAS%B zMg)1UWH~Th?n}4RP;_F}1*CobJKmdY^JS#Zto`Ytvj|rkGTXbXA5{udV3^(V_9nFJ zM%adTu`luFAzp*3{6`_CyPG%om*z>;8r$cCK=t+vLKb`e>GHy-3qZxC7dGMenO3|c znQC_sK1#9yY1_<(J@5-QUd&KyBSmjUHPmg@j?E*`E{7}RDHH@f1=g+7b=ZMF1x zBy(NTdg=)(#X?P2Cirw~UQC;Vo_MvDMlBmj&|13D!(5N6uMY^M5!@y*@YWR3K&|dN zW!XTlc<6FA+mD1;F~fZH$~%i|I`QVgD{N(7EZsPJJZKaQ+|0$e$!^HORDhWWd7MN6 zOADE3?dmSLY5Cv2I?@P3tJlr-TFVd8F!OHGM8M|Y7J8tUer!wIsyG1Scuwh-7m!qk zgMPV@zls%buF(+m=`d@EAd3^7E2yBB2^Vi&9<;g)J}CLPoj$6xJ}ij8hkYjyOL zaGX3i%i$qF?(~dm@J?I7NEEqF7als2>#_<~S^Ky3DrDM2r%b-Y~YQHga;dN3zT2R$VxG*FuuiXCZF_P4L z{&Y``F6RKFu6I2!(DCG0?L5#bTbH|JKpTzD4Q#cy9H#_`R?d_slvT-J%+8?B+^T$L zMJT;4RM=@%X4_I_KS}nf7GGvDS*Gm#@Y6kSjz^TL)WP*jHf@$DO}H>|ufd!kPYEw? z*_^)|;Y}&E5hbgB`k`NhnmFB-ZI2oXc83@z#oVZsx67x*RlYjpUiyS@h;aW@?3k8= zg_d-i5=KKaN_X%|;zhtWhPk2n;qWuN?;BfryDNFtXV-%gR9-#+B30TLdrRpt3z!^X zm*38SRUQi|BH3z5-cd*wITBr<9TuWBoW@a-WT$MlL-o**ObLz9Y-9XHa-8@lkg46y z3sECM>`K=i*z3h#rWbj2OpfR!=bE;=UVi)`P26Z3W4*zi(my5~lze+s_X-Y^bFqQW z4Nq_69=14C;6$wy+_Vbbm*2Z_iEdAtyQ*Wq{z!C(!zj*u>hCR*{Onqq+NWU%yHam4 zOa87hr~ zJICeQ{0*Wo!j~UI0OxhPobKlLihSuCE>Bnlp7&$8E`5Oop*&zs{k80M67qY4-=SRz zBDeZIU7s@J!~eytH=e%czL5D+vsS`9^m|ECcQnw1H1K-h`8o^pv@`W0%Z1D&Q5b&+ zzVUKXL;SLz*k+9#NPsxf#`f+JzuGByH7B~1HK%)eKaG$9lxqE7)=p#R{a>c?|0!Ma z|2G)dUz(2IiGavikbuiCO(wM><}#%NvjGtv=0aUI^o2ame<+v*F3+Zyc8MCQiQ6(i zqTK;Dz-TFK4A!0TB~~Hz1wi+S1i8~IPjaKZ4o>dyOs=fSZ8a| z;6f{1bU6ywoU0$dj^ibA6E$_2^3W5q7!U1wlWB!~P~x=tH+XKNf7(EQ-a_cPh`KW{ zU9qe}>`bD|-ul|$6K;8psUNMftdRFrpC`_|b%+&Rd-Hu3aNcd{Zjr`ha9`MJ%b1aK z%}n}_^NxJXHL0TvJGrn!4cs4m>@ibFMs0R0F!M>CB5Pp{J1Xw6AN+P}s~-uYc!+e| z2VP}o`bi2i{*|Eq>m+Pj??W1SPfLbQ8h1P_b9PaEUW|phD;4T)-ZL+gegx^vHWiTu z3Ec62_aHI8R=^_3BZ-=aee9{XeTVqdFv9-+pGR?@f9e8leL*#07g)~9jk zA*RwV#Vn%$Fyd-PDwo)3wdrmFW}VL~H3dob3LX!Ns!$>X_fzDA;GsGDR@a8r<)rK- zd4jZXf24PIf+SfBxH(q^0MNZFvPj+{h85vRA{lC;P3vch$UDTnQbf`@VPfZ;9)h3- zAy*GT*D9wJAK3r$YHt*Gp@D$-8(f#$;3u=1WORYfYs;-sR%^D#Wx>0BVvS~RX7h_P zdX>0dmBn&T`CUc#tAW;+c^>oGl`tACu7#=RjFN-3S!{H%LpLke9KkbI2b-GgQt z()@6(&BtEM`u8#e8W%C{Aq(74W4Z9wAM|llY7qhB4C;uY-!rEsKGtW54Vt+BV{OPO zVO>(7pcATXKCvumsNPMR$lSy|6R&3l{L>vy$x=iA@SvngU86QdGoy3j@!xbLg_;!D zbQ)&LwptyFc5lN_eHFzMlRoyZKbn>`*RjJu=nv_9=(1HS;S@wFlwVl1PgtlC@wmfz zz_YHa%zvV~T6obi@j_5u^`BO|0Z%SK>H8e_)DS0P$Jw6Ja_MV8NLzz-9h~efpPHI! z?JEKs&J%tP0!*H10?5x^-uF{SadvgD{fGhM6d(VA>@yRku=QKETNiTn$pQL~()ou& zu@?h!AQIg}HN7&IRFTaz8w))Eo-WG@@2OaI8OdI%O@KJ?MbSuI4r~=We#Gi;7)G6- zbSf9@XlXXjrvBy@4tp%`0&WyZGDYyQ4???q6fp`GLu^1t$q+Z3Y!MRDbva(#j6?}> z@3L-G0Caw~J$bukt&_R3pTH%l$Upu~YK-w2G8N}^4sg@^U5=L9ZVA1R|FZo2%W$6? zK@WN)9Q-BVfsXn~IxYcVT{T=6S+d)VM~VaJe@$YYdK=6#{&3FoEOC_m$%4jnJ`SMg zGLWriicVk2pRl};87u(+n{GD&Wb?D{qk+y-Az>#T@Na9a2R2S2xX%K6Bcj!!`>=_! zamig>0PVsG7tWN*uvhp?Hp*VSVT4szBrRerp}p?*`sSuow+um0<=qLqV^t^Zj9T4+ z&De1p&{U%;3VexF4_Ixm9;0>L6LRvW1*E@vwK(zxQjEYy zh@3$y;rIr%3aXbxcl_BM9$?+gRh#wq{N#BB60iNDwMKx6$CZ<>*!Njt9T%wfiu-*bL%)Zy9nWh)Y^F_|CH zDH3KujU8;GFo;>)T?XBe;Mtf3-A5U{YAj9<4oP+fF{`==h=7(q3A57YQlfDWccWA)wr!4a@}7fmdNmWb z3T_Jpg`Bk8c*X!V_gtfzELx<2o&v0|duqW&CKqHKSju0l+AR2|Cj^5h1o%lyk%I*Q z@^f2G3@L4$1IVl+2}e%825UthYE7R8hS^!BqPOl$q2eg(6WGBMp<@8ODGMM-Q6liM zJ%qd>E8vY_YvNmcu)8Z>)@o?Q6^cssoJCIggr)l(Kcy{Gmw%kmq`JCL&H%$gEK{d= zd$x-_&L8=pjHjexnjCRhhb~QBLB~d>$&rKI7}1m#29)GL-o4+hGj6CVKQYyx3Zc_+ zlY||YLv5hcnXT#OEsv~5P5JrME0zgHS* zF*S^?k3M$JSZE~{{9RIP{g80Q5ra|;8g<$4o;>Igv473d<%n5OF0$AJdFQ;ann(bU zgQzcWN|7QKKNUESFXOhzW)O;XXuIVp2#mfDBqr(CzWpoBK|o;WGS{*_)=mM0FW&+K}^0%Rd%ezC_wr?BrU@$In+? z_b1Y}dsbK34|M~lyphn~NqVPRD3#EzC}`JEXTL>bVsE9#ZtZ!>@ewBq)v&@ZjgP%} zg5Oy^Nr%3u#rg(+A2ol;ED!Q%`wi~YFY!mc<7sY>P+}9;a%E9}yv2ron96oMT&oxR zWCm$WgiuZ!fn(^l3&%s`ZodJX_caIICd3Q57RS~7qhUX`H|}b{y@$8f0w#2V^oUseH@6*juYDtKd)~wnfUpdp4$yz+hye2 z_g!Ag-kF?Wf!UF0mxE)xh+N8xo$7@$iEWqt<5}z=McP)bR4ehB@XF|4Zc^p`DvezCXhuZAGd$ z;&#f~;?>qtqhkxm;@H6N&7p@fIJFnPMBUiN#r?hJ?3*qQ@zL``3+$lzKkmTFoz6TG z!he%!dwN}w!?siX5}zC~OogrbAY7>NG9a^wS9WOhKudlix~GnFT&%^)YLxUIe!8k~ZB z`JLP!I9t|pJ6qhAyma|3VuR2hHB-oSjv~H5TDjAl(;caOxCa(4($${b){iLXc`v^D zA_3>d?`(rL8!EqT);E%r6rm7#{I_lp$@_H)WbptO63HKe?~s88gtJg1CIUs?M~jQq6vbm`DO zjot=x9u3|E$CdoJ zu}SF~B#*SIN%jZ_{gjRCJ%=LL*k--cRy4Q$NIb`VBPn~<{Q`00=qBryrI*JwB$2+% zoU0nAybNS|BUhC^4Jk}1Xk^tY-8JyAw4hR1L7(eBM%kwO=}%DfV6p%FE(J1ADLkPS_J?$={#vJ;qDnrC_dF zM>LFIkS-E=<`^Dl$vV}Fhvn9fNE>r0N#AV>zJj1UcxC- zu8orH2Cpz=Uk7+3IK78=)J=_{&j3>&p-J0~p4XzL%IacKW)QdNJ{XY2A`n^H|sAEh>#Y6h;@O*^8UTU!Fe>?N&K|E63MU&45f4=!^drx~Us&NcYU{lRk|HhCT_)UM~ zvgLbNy}K(HVmZ-p&I8YdDt+a04V@LcWD`e+k#jLSTCWlBkJp5)NH@>=Pgx`G@$5W# z%n?@{vhiK`j&QLA%j|h_94ZOAe^gVIb@yqh`0r81d3#b6x92k*;G0+E1idXiWQT*a zxUA?q-rELSM$du?gC|+~6y3^;w=$Ca!gX+418s`COwlBhn*}?U(2X$m2$ZZ7EfA2X zL+XXFH3>L7CFbx1kY>6Azuc)ANFdBchW#0w018pm!LAB zFAM|3J93BcXs5rF`PUP$gFG$LG8+z%MPW^<9P_0&9dwVo&hpNFVd3P7QzzJh_X-n_ zmJu+>UdzuXAKN|ZERVuT?0T)@@Jw^S$)}{_-#7a-++U;sKEtV^*#e0eIVjar~Y&!n2@=cV_@l&=u<8y->uwA|_244QLB2 zALGS5K= 0: + return self.order[i - 1] + else: + return self + + def has_previous(self): + """Check if there is a previous step in the collection process""" + return self.previous() != self + + +class GeoEstimatorWidget(QtWidgets.QWidget, geo_estimator_widget_class): + """Widget for the geometry estimator UI""" + + _timeout_reader_signal = QtCore.pyqtSignal(object) + _solution_ready_signal = QtCore.pyqtSignal(object) + + def __init__(self, lighthouse_tab): + super(GeoEstimatorWidget, self).__init__() + self.setupUi(self) + + self._lighthouse_tab = lighthouse_tab + self._helper = lighthouse_tab._helper + + self._step_next_button.clicked.connect(lambda: self._update_step(self._current_step.next())) + self._step_previous_button.clicked.connect(lambda: self._update_step(self._current_step.previous())) + self._step_measure.clicked.connect(self._measure) + + self._timeout_reader = TimeoutAngleReader(self._helper.cf, self._timeout_reader_signal.emit) + self._timeout_reader_signal.connect(self._average_available_cb) + self._timeout_reader_result_setter = None + + self._current_step = _CollectionStep.ORIGIN + self._populate_step() + self._update_ui_reading(False) + + self._container = LhGeoInputContainer(LhDeck4SensorPositions.positions) + self._solution_ready_signal.connect(self._solution_ready_cb) + self._solver_thread = None + + # TODO krri handĺe disconnects + + def setVisible(self, visible: bool): + super(GeoEstimatorWidget, self).setVisible(visible) + if visible: + if self._solver_thread is None: + logger.info("Starting solver thread") + self._solver_thread = LhGeoEstimationManager.SolverThread(self._container, + is_done_cb=self._solution_ready_signal.emit) + self._solver_thread.start() + else: + if self._solver_thread is not None: + logger.info("Stopping solver thread") + self._solver_thread.stop(do_join=False) + self._solver_thread = None + + def _update_step(self, step): + """Update the widget to display the new step""" + if step != self._current_step: + self._current_step = step + self._populate_step() + + def _populate_step(self): + """Populate the widget with the current step's information""" + step = self._current_step + + self._step_title.setText(step.title) + self._step_image.setPixmap(QtGui.QPixmap( + cfclient.module_path + '/ui/widgets/geo_estimator_resources/' + step.image)) + self._step_instructions.setText(step.instructions) + self._step_info.setText('') + + self._step_measure.setVisible(step != _CollectionStep.XYZ_SPACE) + + self._step_previous_button.setEnabled(step.has_previous()) + self._step_next_button.setEnabled(step.has_next()) + + def _update_ui_reading(self, is_reading: bool): + """Update the UI to reflect whether a reading is in progress""" + is_enabled = not is_reading + + self._step_measure.setEnabled(is_enabled) + self._step_next_button.setEnabled(is_enabled) + self._step_previous_button.setEnabled(is_enabled) + + def _measure(self): + """Trigger the measurement for the current step""" + match self._current_step: + case _CollectionStep.ORIGIN: + self._measure_origin() + case _CollectionStep.X_AXIS: + self._measure_x_axis() + case _CollectionStep.XY_PLANE: + self._measure_xy_plane() + case _CollectionStep.XYZ_SPACE: + pass + + def _measure_origin(self): + """Measure the origin position""" + # Placeholder for actual measurement logic + logger.info("Measuring origin position...") + self._start_timeout_average_read(self._container.set_origin_sample) + + def _measure_x_axis(self): + """Measure the X-axis position""" + # Placeholder for actual measurement logic + logger.info("Measuring X-axis position...") + self._start_timeout_average_read(self._container.set_x_axis_sample) + + def _measure_xy_plane(self): + """Measure the XY-plane position""" + # Placeholder for actual measurement logic + logger.info("Measuring XY-plane position...") + self._start_timeout_average_read(self._container.append_xy_plane_sample) + + def _start_timeout_average_read(self, setter: Callable[[LhCfPoseSample], None]): + """Start the timeout average angle reader""" + self._timeout_reader.start() + self._timeout_reader_result_setter = setter + self._step_info.setText("Collecting angles...") + self._update_ui_reading(True) + + def _average_available_cb(self, sample: LhCfPoseSample): + """Callback for when the average angles are available from the reader or after""" + + bs_ids = list(sample.angles_calibrated.keys()) + bs_ids.sort() + bs_seen = ', '.join(map(lambda x: str(x + 1), bs_ids)) + bs_count = len(bs_ids) + + logger.info("Average angles received: %s", bs_seen) + + self._update_ui_reading(False) + + if bs_count == 0: + self._step_info.setText("No base stations seen, please try again.") + elif bs_count < 2: + self._step_info.setText(f"Only one base station (nr {bs_seen}) was seen, we need at least two. Please try again.") + else: + if self._timeout_reader_result_setter is not None: + self._timeout_reader_result_setter(sample) + self._step_info.setText(f"Base stations {bs_seen} were seen. Sample stored.") + + self._timeout_reader_result_setter = None + + def _solution_ready_cb(self, solution: LighthouseGeometrySolution): + logger.info('Solution ready --------------------------------------') + logger.info(f'Converged: {solution.has_converged}') + logger.info(f'Progress info: {solution.progress_info}') + logger.info(f'Progress is ok: {solution.progress_is_ok}') + logger.info(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}') + logger.info(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}') + logger.info(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}') + logger.info(f'XYZ space: {solution.xyz_space_samples_info}') + logger.info(f'General info: {solution.general_failure_info}') + + if solution.progress_is_ok: + self._upload_geometry(solution.poses.bs_poses) + + def _upload_geometry(self, bs_poses: dict[int, LighthouseBsGeometry]): + geo_dict = {} + for bs_id, pose in bs_poses.items(): + geo = LighthouseBsGeometry() + geo.origin = pose.translation.tolist() + geo.rotation_matrix = pose.rot_matrix.tolist() + geo.valid = True + geo_dict[bs_id] = geo + + logger.info('Uploading geometry to Crazyflie') + self._lighthouse_tab.write_and_store_geometry(geo_dict) + + +class TimeoutAngleReader: + def __init__(self, cf: Crazyflie, ready_cb: Callable[[LhCfPoseSample], None]): + self._ready_cb = ready_cb + + self.timeout_timer = QtCore.QTimer() + self.timeout_timer.timeout.connect(self._timeout_cb) + self.timeout_timer.setSingleShot(True) + + self.reader = LighthouseSweepAngleAverageReader(cf, self._reader_ready_cb) + + self.lock = threading.Lock() + self.is_collecting = False + + def start(self, timeout=2000): + with self.lock: + if self.is_collecting: + raise RuntimeError("Measurement already in progress!") + self.is_collecting = True + + self.reader.start_angle_collection() + self.timeout_timer.start(timeout) + logger.info("Starting angle collection with timeout of %d ms", timeout) + + def _timeout_cb(self): + logger.info("Timeout reached, stopping angle collection") + with self.lock: + if not self.is_collecting: + return + self.is_collecting = False + + self.reader.stop_angle_collection() + + result = LhCfPoseSample({}) + self._ready_cb(result) + + def _reader_ready_cb(self, recorded_angles: dict[int, tuple[int, LighthouseBsVectors]]): + logger.info("Reader ready with %d base stations", len(recorded_angles)) + with self.lock: + if not self.is_collecting: + return + self.is_collecting = False + + # TODO krri Can not stop the timer from this thread + # self.timeout_timer.stop() + + angles_calibrated: dict[int, LighthouseBsVectors] = {} + for bs_id, data in recorded_angles.items(): + angles_calibrated[bs_id] = data[1] + + result = LhCfPoseSample(angles_calibrated) + self._ready_cb(result) From 00909f35bbf9356fa60b2bb1681f1913467e43e0 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 25 Jun 2025 11:35:37 +0200 Subject: [PATCH 11/18] Improved used feedback --- src/cfclient/ui/widgets/geo_estimator.ui | 110 +++++++++++++++++- .../ui/widgets/geo_estimator_widget.py | 97 ++++++++++++--- 2 files changed, 191 insertions(+), 16 deletions(-) diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui index 003bdc5b..f7c5d765 100644 --- a/src/cfclient/ui/widgets/geo_estimator.ui +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -7,7 +7,7 @@ 0 0 400 - 300 + 753 @@ -54,7 +54,7 @@ - + TextLabel @@ -73,6 +73,16 @@ + + + + QFrame::Panel + + + TextLabel + + + @@ -113,6 +123,102 @@ + + + + + + + 0 + 0 + + + + Data status + + + + + + Origin + + + + + + + X-axis + + + + + + + XY-plane + + + + + + + + 0 + 0 + + + + XYZ-space + + + + + + + + + + + 0 + 0 + + + + Solution status + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + + + + + + Clear all + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index 093a2d4d..dbaa0bb8 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -31,6 +31,8 @@ from typing import Callable from PyQt6 import QtCore, QtWidgets, uic, QtGui +from PyQt6.QtWidgets import QMessageBox + import logging from enum import Enum @@ -126,6 +128,12 @@ def has_previous(self): return self.previous() != self +STYLE_GREEN_BACKGROUND = "background-color: lightgreen;" +STYLE_RED_BACKGROUND = "background-color: lightpink;" + + +# TODO krri Sample XYZ-space + class GeoEstimatorWidget(QtWidgets.QWidget, geo_estimator_widget_class): """Widget for the geometry estimator UI""" @@ -143,20 +151,23 @@ def __init__(self, lighthouse_tab): self._step_previous_button.clicked.connect(lambda: self._update_step(self._current_step.previous())) self._step_measure.clicked.connect(self._measure) + self._clear_all_button.clicked.connect(self._clear_all) + self._timeout_reader = TimeoutAngleReader(self._helper.cf, self._timeout_reader_signal.emit) self._timeout_reader_signal.connect(self._average_available_cb) self._timeout_reader_result_setter = None + self._container = LhGeoInputContainer(LhDeck4SensorPositions.positions) + + self._latest_solution: LighthouseGeometrySolution = LighthouseGeometrySolution() + self._current_step = _CollectionStep.ORIGIN self._populate_step() self._update_ui_reading(False) - self._container = LhGeoInputContainer(LhDeck4SensorPositions.positions) self._solution_ready_signal.connect(self._solution_ready_cb) self._solver_thread = None - # TODO krri handĺe disconnects - def setVisible(self, visible: bool): super(GeoEstimatorWidget, self).setVisible(visible) if visible: @@ -171,6 +182,19 @@ def setVisible(self, visible: bool): self._solver_thread.stop(do_join=False) self._solver_thread = None + def clear_state(self): + self._container.clear_all_samples() + + def _clear_all(self): + dlg = QMessageBox(self) + dlg.setWindowTitle("Clear samples Confirmation") + dlg.setText("Are you sure you want to clear all samples and start over?") + dlg.setStandardButtons(QMessageBox.StandardButton.Yes | QMessageBox.StandardButton.No) + button = dlg.exec() + + if button == QMessageBox.StandardButton.Yes: + self.clear_state() + def _update_step(self, step): """Update the widget to display the new step""" if step != self._current_step: @@ -188,18 +212,60 @@ def _populate_step(self): self._step_info.setText('') self._step_measure.setVisible(step != _CollectionStep.XYZ_SPACE) + self._update_solution_info() self._step_previous_button.setEnabled(step.has_previous()) self._step_next_button.setEnabled(step.has_next()) def _update_ui_reading(self, is_reading: bool): - """Update the UI to reflect whether a reading is in progress""" + """Update the UI to reflect whether a reading is in progress, that is enable/disable buttons""" is_enabled = not is_reading self._step_measure.setEnabled(is_enabled) self._step_next_button.setEnabled(is_enabled) self._step_previous_button.setEnabled(is_enabled) + def _update_solution_info(self): + solution = self._latest_solution + + match self._current_step: + case _CollectionStep.ORIGIN: + self._step_solution_info.setText( + 'OK' if solution.is_origin_sample_valid else solution.origin_sample_info) + case _CollectionStep.X_AXIS: + self._step_solution_info.setText( + 'OK' if solution.is_x_axis_samples_valid else solution.x_axis_samples_info) + case _CollectionStep.XY_PLANE: + self._step_solution_info.setText( + 'OK' if solution.is_xy_plane_samples_valid else solution.xy_plane_samples_info) + case _CollectionStep.XYZ_SPACE: + self._step_solution_info.setText(solution.xyz_space_samples_info) + + self._set_background_color(self._data_status_origin, solution.is_origin_sample_valid) + self._set_background_color(self._data_status_x_axis, solution.is_x_axis_samples_valid) + self._set_background_color(self._data_status_xy_plane, solution.is_xy_plane_samples_valid) + # TODO krri XYZ-space + + if solution.progress_is_ok: + self._solution_status_is_ok.setText('Solution is OK') + self._solution_status_uploaded.setText('Uploaded') + else: + self._solution_status_is_ok.setText('No solution') + self._solution_status_uploaded.setText('Not uploaded') + self._set_background_color(self._solution_status_is_ok, solution.progress_is_ok) + + self._solution_status_info.setText(solution.general_failure_info) + + def _set_background_color(self, widget: QtWidgets.QWidget, is_valid: bool): + """Set the background color of a widget based on validity""" + if is_valid: + widget.setStyleSheet(STYLE_GREEN_BACKGROUND) + else: + widget.setStyleSheet(STYLE_RED_BACKGROUND) + + # Force a repaint to ensure the style is applied immediately + widget.repaint() + def _measure(self): """Trigger the measurement for the current step""" match self._current_step: @@ -261,15 +327,18 @@ def _average_available_cb(self, sample: LhCfPoseSample): self._timeout_reader_result_setter = None def _solution_ready_cb(self, solution: LighthouseGeometrySolution): - logger.info('Solution ready --------------------------------------') - logger.info(f'Converged: {solution.has_converged}') - logger.info(f'Progress info: {solution.progress_info}') - logger.info(f'Progress is ok: {solution.progress_is_ok}') - logger.info(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}') - logger.info(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}') - logger.info(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}') - logger.info(f'XYZ space: {solution.xyz_space_samples_info}') - logger.info(f'General info: {solution.general_failure_info}') + self._latest_solution = solution + self._update_solution_info() + + logger.debug('Solution ready --------------------------------------') + logger.debug(f'Converged: {solution.has_converged}') + logger.debug(f'Progress info: {solution.progress_info}') + logger.debug(f'Progress is ok: {solution.progress_is_ok}') + logger.debug(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}') + logger.debug(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}') + logger.debug(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}') + logger.debug(f'XYZ space: {solution.xyz_space_samples_info}') + logger.debug(f'General info: {solution.general_failure_info}') if solution.progress_is_ok: self._upload_geometry(solution.poses.bs_poses) @@ -329,7 +398,7 @@ def _reader_ready_cb(self, recorded_angles: dict[int, tuple[int, LighthouseBsVec return self.is_collecting = False - # TODO krri Can not stop the timer from this thread + # Can not stop the timer from this thread, let it run. # self.timeout_timer.stop() angles_calibrated: dict[int, LighthouseBsVectors] = {} From 0366878b1aa4cafe27ac5836c7601aaca777ceff Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 25 Jun 2025 12:11:56 +0200 Subject: [PATCH 12/18] Added samplinf of xyz-space --- src/cfclient/ui/tabs/lighthouse_tab.py | 4 +- .../ui/widgets/geo_estimator_widget.py | 49 +++++++++++++------ 2 files changed, 37 insertions(+), 16 deletions(-) diff --git a/src/cfclient/ui/tabs/lighthouse_tab.py b/src/cfclient/ui/tabs/lighthouse_tab.py index 6af2430c..17068a11 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.py +++ b/src/cfclient/ui/tabs/lighthouse_tab.py @@ -268,6 +268,7 @@ class UiMode(Enum): flying = 1 geo_estimation = 2 + class LighthouseTab(TabToolbox, lighthouse_tab_class): """Tab for plotting Lighthouse data""" @@ -306,7 +307,6 @@ def __init__(self, helper): self._geo_estimator_widget = GeoEstimatorWidget(self) self._geometry_area.addWidget(self._geo_estimator_widget) - # Always wrap callbacks from Crazyflie API though QT Signal/Slots # to avoid manipulating the UI when rendering it self._connected_signal.connect(self._connected) @@ -374,7 +374,7 @@ def __init__(self, helper): self._update_ui() def write_and_store_geometry(self, geometries: dict[int, LighthouseBsGeometry]): - # TODO krri Hanlde repeated quick writes. This is called from the geo wizard and write_and_store_config() will + # TODO krri Handle repeated quick writes. This is called from the geo wizard and write_and_store_config() will # throw if there is an ongoing write if self._lh_config_writer: self._lh_config_writer.write_and_store_config(self._new_system_config_written_to_cf_signal.emit, diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index dbaa0bb8..7208c5fb 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -132,8 +132,6 @@ def has_previous(self): STYLE_RED_BACKGROUND = "background-color: lightpink;" -# TODO krri Sample XYZ-space - class GeoEstimatorWidget(QtWidgets.QWidget, geo_estimator_widget_class): """Widget for the geometry estimator UI""" @@ -147,8 +145,8 @@ def __init__(self, lighthouse_tab): self._lighthouse_tab = lighthouse_tab self._helper = lighthouse_tab._helper - self._step_next_button.clicked.connect(lambda: self._update_step(self._current_step.next())) - self._step_previous_button.clicked.connect(lambda: self._update_step(self._current_step.previous())) + self._step_next_button.clicked.connect(lambda: self._change_step(self._current_step.next())) + self._step_previous_button.clicked.connect(lambda: self._change_step(self._current_step.previous())) self._step_measure.clicked.connect(self._measure) self._clear_all_button.clicked.connect(self._clear_all) @@ -157,13 +155,17 @@ def __init__(self, lighthouse_tab): self._timeout_reader_signal.connect(self._average_available_cb) self._timeout_reader_result_setter = None + self._action_detector = UserActionDetector(self._helper.cf, cb=self._user_action_detected_cb) + self._matched_reader = LighthouseMatchedSweepAngleReader(self._helper.cf, self._single_sample_ready_cb) + self._container = LhGeoInputContainer(LhDeck4SensorPositions.positions) self._latest_solution: LighthouseGeometrySolution = LighthouseGeometrySolution() self._current_step = _CollectionStep.ORIGIN - self._populate_step() + self._update_step_ui() self._update_ui_reading(False) + self._update_solution_info() self._solution_ready_signal.connect(self._solution_ready_cb) self._solver_thread = None @@ -177,6 +179,7 @@ def setVisible(self, visible: bool): is_done_cb=self._solution_ready_signal.emit) self._solver_thread.start() else: + self._action_detector.stop() if self._solver_thread is not None: logger.info("Stopping solver thread") self._solver_thread.stop(do_join=False) @@ -195,13 +198,17 @@ def _clear_all(self): if button == QMessageBox.StandardButton.Yes: self.clear_state() - def _update_step(self, step): + def _change_step(self, step): """Update the widget to display the new step""" if step != self._current_step: self._current_step = step - self._populate_step() + self._update_step_ui() + if step == _CollectionStep.XYZ_SPACE: + self._action_detector.start() + else: + self._action_detector.stop() - def _populate_step(self): + def _update_step_ui(self): """Populate the widget with the current step's information""" step = self._current_step @@ -212,11 +219,12 @@ def _populate_step(self): self._step_info.setText('') self._step_measure.setVisible(step != _CollectionStep.XYZ_SPACE) - self._update_solution_info() self._step_previous_button.setEnabled(step.has_previous()) self._step_next_button.setEnabled(step.has_next()) + self._update_solution_info() + def _update_ui_reading(self, is_reading: bool): """Update the UI to reflect whether a reading is in progress, that is enable/disable buttons""" is_enabled = not is_reading @@ -236,15 +244,20 @@ def _update_solution_info(self): self._step_solution_info.setText( 'OK' if solution.is_x_axis_samples_valid else solution.x_axis_samples_info) case _CollectionStep.XY_PLANE: - self._step_solution_info.setText( - 'OK' if solution.is_xy_plane_samples_valid else solution.xy_plane_samples_info) + if solution.xy_plane_samples_info: + text = f'OK, {self._container.xy_plane_sample_count()} sample(s)' + else: + text = solution.xy_plane_samples_info + self._step_solution_info.setText(text) case _CollectionStep.XYZ_SPACE: - self._step_solution_info.setText(solution.xyz_space_samples_info) + text = f'OK, {self._container.xyz_space_sample_count()} sample(s)' + if solution.xyz_space_samples_info: + text += f', {solution.xyz_space_samples_info}' + self._step_solution_info.setText(text) self._set_background_color(self._data_status_origin, solution.is_origin_sample_valid) self._set_background_color(self._data_status_x_axis, solution.is_x_axis_samples_valid) self._set_background_color(self._data_status_xy_plane, solution.is_xy_plane_samples_valid) - # TODO krri XYZ-space if solution.progress_is_ok: self._solution_status_is_ok.setText('Solution is OK') @@ -318,7 +331,8 @@ def _average_available_cb(self, sample: LhCfPoseSample): if bs_count == 0: self._step_info.setText("No base stations seen, please try again.") elif bs_count < 2: - self._step_info.setText(f"Only one base station (nr {bs_seen}) was seen, we need at least two. Please try again.") + self._step_info.setText(f"Only one base station (nr {bs_seen}) was seen, " + + "we need at least two. Please try again.") else: if self._timeout_reader_result_setter is not None: self._timeout_reader_result_setter(sample) @@ -355,6 +369,13 @@ def _upload_geometry(self, bs_poses: dict[int, LighthouseBsGeometry]): logger.info('Uploading geometry to Crazyflie') self._lighthouse_tab.write_and_store_geometry(geo_dict) + def _user_action_detected_cb(self): + self._matched_reader.start() + + def _single_sample_ready_cb(self, sample: LhCfPoseSample): + self._container.append_xyz_space_samples([sample]) + self._update_solution_info() + class TimeoutAngleReader: def __init__(self, cf: Crazyflie, ready_cb: Callable[[LhCfPoseSample], None]): From 94db0bdd962a1dbcc951d4520171a6587503a382 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 27 Jun 2025 09:07:59 +0200 Subject: [PATCH 13/18] Use signal for callback from cflib --- src/cfclient/ui/widgets/geo_estimator_widget.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index 7208c5fb..8c442c16 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -78,7 +78,7 @@ class _CollectionStep(Enum): 'This position is used to map the the XY-plane to the floor.\n' + 'You can sample multiple positions to get a more precise definition.') XYZ_SPACE = ('bslh_4.png', - 'Step 4. Flight space', + 'Step 4. XYZ-space', 'Sample points in the space that will be used.\n' + 'Make sure all the base stations are received, you need at least two base \n' + 'stations in each sample. Sample by rotating the Crazyflie quickly \n' + @@ -136,6 +136,7 @@ class GeoEstimatorWidget(QtWidgets.QWidget, geo_estimator_widget_class): """Widget for the geometry estimator UI""" _timeout_reader_signal = QtCore.pyqtSignal(object) + _container_updated_signal = QtCore.pyqtSignal() _solution_ready_signal = QtCore.pyqtSignal(object) def __init__(self, lighthouse_tab): @@ -155,6 +156,8 @@ def __init__(self, lighthouse_tab): self._timeout_reader_signal.connect(self._average_available_cb) self._timeout_reader_result_setter = None + self._container_updated_signal.connect(self._update_solution_info) + self._action_detector = UserActionDetector(self._helper.cf, cb=self._user_action_detected_cb) self._matched_reader = LighthouseMatchedSweepAngleReader(self._helper.cf, self._single_sample_ready_cb) @@ -374,7 +377,8 @@ def _user_action_detected_cb(self): def _single_sample_ready_cb(self, sample: LhCfPoseSample): self._container.append_xyz_space_samples([sample]) - self._update_solution_info() + self._container_updated_signal.emit() + # self._update_solution_info() class TimeoutAngleReader: From f7860e4c1b45008e44ba642e7882945db1d2447c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 27 Jun 2025 09:51:03 +0200 Subject: [PATCH 14/18] Added basic link stats --- src/cfclient/ui/widgets/geo_estimator.ui | 12 ++++++++++++ src/cfclient/ui/widgets/geo_estimator_widget.py | 10 ++++++++++ 2 files changed, 22 insertions(+) diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui index f7c5d765..b1dece7d 100644 --- a/src/cfclient/ui/widgets/geo_estimator.ui +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -212,6 +212,18 @@ + + + + Base station links + + + + + + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index 8c442c16..c601f4fc 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -272,6 +272,16 @@ def _update_solution_info(self): self._solution_status_info.setText(solution.general_failure_info) + link_info = '' + for bs, link_map in solution.link_count.items(): + count = len(link_map) + if count > 0: + seen_bs = ', '.join(map(lambda x: str(x + 1), link_map.keys())) + link_info += f'Base station {bs + 1}: {count} link(s), id {seen_bs}\n' + else: + link_info += f'Base station {bs + 1}: No link\n' + self._bs_link_text.setPlainText(link_info) + def _set_background_color(self, widget: QtWidgets.QWidget, is_valid: bool): """Set the background color of a widget based on validity""" if is_valid: From e78d7eb11f72feebeced5d10a817e7d698321b7a Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 3 Jul 2025 10:28:56 +0200 Subject: [PATCH 15/18] Add user feedback when sampling --- src/cfclient/ui/widgets/geo_estimator.ui | 53 ++++++++++++++----- .../ui/widgets/geo_estimator_widget.py | 52 ++++++++++++++++-- 2 files changed, 89 insertions(+), 16 deletions(-) diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui index b1dece7d..42254895 100644 --- a/src/cfclient/ui/widgets/geo_estimator.ui +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -27,7 +27,10 @@ - + + + + Sample collection @@ -41,6 +44,9 @@ + + + Image @@ -138,37 +144,55 @@ - + + + + 0 + 0 + + + + + Origin + + false + - + + + + X-axis + + false + - + XY-plane + + false + - - - - 0 - 0 - - + XYZ-space + + false + @@ -219,7 +243,12 @@ - + + + qwe + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index c601f4fc..264ce7c1 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -32,6 +32,7 @@ from typing import Callable from PyQt6 import QtCore, QtWidgets, uic, QtGui from PyQt6.QtWidgets import QMessageBox +from PyQt6.QtCore import QTimer import logging @@ -128,8 +129,15 @@ def has_previous(self): return self.previous() != self +class _UserNotificationType(Enum): + SUCCESS = "success" + FAILURE = "failure" + PENDING = "pending" + + STYLE_GREEN_BACKGROUND = "background-color: lightgreen;" STYLE_RED_BACKGROUND = "background-color: lightpink;" +STYLE_YELLOW_BACKGROUND = "background-color: lightyellow;" class GeoEstimatorWidget(QtWidgets.QWidget, geo_estimator_widget_class): @@ -137,6 +145,7 @@ class GeoEstimatorWidget(QtWidgets.QWidget, geo_estimator_widget_class): _timeout_reader_signal = QtCore.pyqtSignal(object) _container_updated_signal = QtCore.pyqtSignal() + _user_notification_signal = QtCore.pyqtSignal(object) _solution_ready_signal = QtCore.pyqtSignal(object) def __init__(self, lighthouse_tab): @@ -158,8 +167,14 @@ def __init__(self, lighthouse_tab): self._container_updated_signal.connect(self._update_solution_info) + self._user_notification_signal.connect(self._notify_user) + self._user_notification_clear_timer = QTimer() + self._user_notification_clear_timer.setSingleShot(True) + self._user_notification_clear_timer.timeout.connect(self._user_notification_clear) + self._action_detector = UserActionDetector(self._helper.cf, cb=self._user_action_detected_cb) - self._matched_reader = LighthouseMatchedSweepAngleReader(self._helper.cf, self._single_sample_ready_cb) + self._matched_reader = LighthouseMatchedSweepAngleReader(self._helper.cf, self._single_sample_ready_cb, + timeout_cb=self._single_sample_timeout_cb) self._container = LhGeoInputContainer(LhDeck4SensorPositions.positions) @@ -173,6 +188,11 @@ def __init__(self, lighthouse_tab): self._solution_ready_signal.connect(self._solution_ready_cb) self._solver_thread = None + self._data_status_origin.clicked.connect(lambda: self._change_step(_CollectionStep.ORIGIN)) + self._data_status_x_axis.clicked.connect(lambda: self._change_step(_CollectionStep.X_AXIS)) + self._data_status_xy_plane.clicked.connect(lambda: self._change_step(_CollectionStep.XY_PLANE)) + self._data_status_xyz_space.clicked.connect(lambda: self._change_step(_CollectionStep.XYZ_SPACE)) + def setVisible(self, visible: bool): super(GeoEstimatorWidget, self).setVisible(visible) if visible: @@ -282,6 +302,23 @@ def _update_solution_info(self): link_info += f'Base station {bs + 1}: No link\n' self._bs_link_text.setPlainText(link_info) + def _notify_user(self, notification_type: _UserNotificationType): + match notification_type: + case _UserNotificationType.SUCCESS: + self._helper.cf.platform.send_user_notification(True) + self._sample_collection_box.setStyleSheet(STYLE_GREEN_BACKGROUND) + case _UserNotificationType.FAILURE: + self._helper.cf.platform.send_user_notification(False) + self._sample_collection_box.setStyleSheet(STYLE_RED_BACKGROUND) + case _UserNotificationType.PENDING: + self._sample_collection_box.setStyleSheet(STYLE_YELLOW_BACKGROUND) + + self._user_notification_clear_timer.stop() + self._user_notification_clear_timer.start(1000) + + def _user_notification_clear(self): + self._sample_collection_box.setStyleSheet('') + def _set_background_color(self, widget: QtWidgets.QWidget, is_valid: bool): """Set the background color of a widget based on validity""" if is_valid: @@ -343,13 +380,16 @@ def _average_available_cb(self, sample: LhCfPoseSample): if bs_count == 0: self._step_info.setText("No base stations seen, please try again.") + self._user_notification_signal.emit(_UserNotificationType.FAILURE) elif bs_count < 2: self._step_info.setText(f"Only one base station (nr {bs_seen}) was seen, " + "we need at least two. Please try again.") + self._user_notification_signal.emit(_UserNotificationType.FAILURE) else: if self._timeout_reader_result_setter is not None: self._timeout_reader_result_setter(sample) self._step_info.setText(f"Base stations {bs_seen} were seen. Sample stored.") + self._user_notification_signal.emit(_UserNotificationType.SUCCESS) self._timeout_reader_result_setter = None @@ -383,12 +423,16 @@ def _upload_geometry(self, bs_poses: dict[int, LighthouseBsGeometry]): self._lighthouse_tab.write_and_store_geometry(geo_dict) def _user_action_detected_cb(self): - self._matched_reader.start() + self._user_notification_signal.emit(_UserNotificationType.PENDING) + self._matched_reader.start(timeout=1.0) def _single_sample_ready_cb(self, sample: LhCfPoseSample): - self._container.append_xyz_space_samples([sample]) + self._user_notification_signal.emit(_UserNotificationType.SUCCESS) self._container_updated_signal.emit() - # self._update_solution_info() + self._container.append_xyz_space_samples([sample]) + + def _single_sample_timeout_cb(self): + self._user_notification_signal.emit(_UserNotificationType.FAILURE) class TimeoutAngleReader: From 8dbe0994bfaf8459fc1ad6cd1e538e5e021a34c6 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 3 Jul 2025 10:40:30 +0200 Subject: [PATCH 16/18] Added button to sample XYZ-space --- .../ui/widgets/geo_estimator_widget.py | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index 264ce7c1..888c4ce7 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -241,7 +241,10 @@ def _update_step_ui(self): self._step_instructions.setText(step.instructions) self._step_info.setText('') - self._step_measure.setVisible(step != _CollectionStep.XYZ_SPACE) + if step == _CollectionStep.XYZ_SPACE: + self._step_measure.setText('Sample position') + else: + self._step_measure.setText('Start measurement') self._step_previous_button.setEnabled(step.has_previous()) self._step_next_button.setEnabled(step.has_next()) @@ -339,26 +342,29 @@ def _measure(self): case _CollectionStep.XY_PLANE: self._measure_xy_plane() case _CollectionStep.XYZ_SPACE: - pass + self._measure_xyz_space() def _measure_origin(self): """Measure the origin position""" - # Placeholder for actual measurement logic - logger.info("Measuring origin position...") + logger.debug("Measuring origin position...") self._start_timeout_average_read(self._container.set_origin_sample) def _measure_x_axis(self): """Measure the X-axis position""" - # Placeholder for actual measurement logic - logger.info("Measuring X-axis position...") + logger.debug("Measuring X-axis position...") self._start_timeout_average_read(self._container.set_x_axis_sample) def _measure_xy_plane(self): """Measure the XY-plane position""" - # Placeholder for actual measurement logic - logger.info("Measuring XY-plane position...") + logger.debug("Measuring XY-plane position...") self._start_timeout_average_read(self._container.append_xy_plane_sample) + def _measure_xyz_space(self): + """Measure the XYZ-space position""" + logger.debug("Measuring XYZ-space position...") + self._user_notification_signal.emit(_UserNotificationType.PENDING) + self._matched_reader.start(timeout=1.0) + def _start_timeout_average_read(self, setter: Callable[[LhCfPoseSample], None]): """Start the timeout average angle reader""" self._timeout_reader.start() @@ -423,8 +429,7 @@ def _upload_geometry(self, bs_poses: dict[int, LighthouseBsGeometry]): self._lighthouse_tab.write_and_store_geometry(geo_dict) def _user_action_detected_cb(self): - self._user_notification_signal.emit(_UserNotificationType.PENDING) - self._matched_reader.start(timeout=1.0) + self._measure_xyz_space() def _single_sample_ready_cb(self, sample: LhCfPoseSample): self._user_notification_signal.emit(_UserNotificationType.SUCCESS) From a58093a0a7d4ad99a1cde469511ad39ba60dffa2 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 3 Jul 2025 15:53:51 +0200 Subject: [PATCH 17/18] Show solution error --- src/cfclient/ui/widgets/geo_estimator.ui | 7 +++++++ src/cfclient/ui/widgets/geo_estimator_widget.py | 2 ++ 2 files changed, 9 insertions(+) diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui index 42254895..ad3aa880 100644 --- a/src/cfclient/ui/widgets/geo_estimator.ui +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -224,6 +224,13 @@ + + + + TextLabel + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index 888c4ce7..b0bb5865 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -288,9 +288,11 @@ def _update_solution_info(self): if solution.progress_is_ok: self._solution_status_is_ok.setText('Solution is OK') self._solution_status_uploaded.setText('Uploaded') + self._solution_status_max_error.setText(f'Error: {solution.error_stats.max * 1000:.1f} mm') else: self._solution_status_is_ok.setText('No solution') self._solution_status_uploaded.setText('Not uploaded') + self._solution_status_max_error.setText('Error: --') self._set_background_color(self._solution_status_is_ok, solution.progress_is_ok) self._solution_status_info.setText(solution.general_failure_info) From 29bef96e24c9d764dfc4e897356bffb3cb5e13aa Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 4 Jul 2025 18:17:42 +0200 Subject: [PATCH 18/18] Added file and session management --- src/cfclient/ui/widgets/geo_estimator.ui | 29 +++++++++- .../ui/widgets/geo_estimator_widget.py | 55 +++++++++++++++++-- 2 files changed, 77 insertions(+), 7 deletions(-) diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui index ad3aa880..5c8ebdfe 100644 --- a/src/cfclient/ui/widgets/geo_estimator.ui +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -261,10 +261,33 @@ - - - Clear all + + + Session management + + + + + Load + + + + + + + Save copy as... + + + + + + + New session + + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py index b0bb5865..bc798cc8 100644 --- a/src/cfclient/ui/widgets/geo_estimator_widget.py +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -29,8 +29,10 @@ Container for the geometry estimation functionality in the lighthouse tab. """ +import os from typing import Callable from PyQt6 import QtCore, QtWidgets, uic, QtGui +from PyQt6.QtWidgets import QFileDialog from PyQt6.QtWidgets import QMessageBox from PyQt6.QtCore import QTimer @@ -148,6 +150,8 @@ class GeoEstimatorWidget(QtWidgets.QWidget, geo_estimator_widget_class): _user_notification_signal = QtCore.pyqtSignal(object) _solution_ready_signal = QtCore.pyqtSignal(object) + FILE_REGEX_YAML = "Config *.yaml;;All *.*" + def __init__(self, lighthouse_tab): super(GeoEstimatorWidget, self).__init__() self.setupUi(self) @@ -160,6 +164,8 @@ def __init__(self, lighthouse_tab): self._step_measure.clicked.connect(self._measure) self._clear_all_button.clicked.connect(self._clear_all) + self._load_button.clicked.connect(self._load_from_file) + self._save_button.clicked.connect(self._save_to_file) self._timeout_reader = TimeoutAngleReader(self._helper.cf, self._timeout_reader_signal.emit) self._timeout_reader_signal.connect(self._average_available_cb) @@ -177,6 +183,8 @@ def __init__(self, lighthouse_tab): timeout_cb=self._single_sample_timeout_cb) self._container = LhGeoInputContainer(LhDeck4SensorPositions.positions) + self._session_path = os.path.join(cfclient.config_path, 'lh_geo_sessions') + self._container.enable_auto_save(self._session_path) self._latest_solution: LighthouseGeometrySolution = LighthouseGeometrySolution() @@ -208,7 +216,7 @@ def setVisible(self, visible: bool): self._solver_thread.stop(do_join=False) self._solver_thread = None - def clear_state(self): + def new_session(self): self._container.clear_all_samples() def _clear_all(self): @@ -219,7 +227,34 @@ def _clear_all(self): button = dlg.exec() if button == QMessageBox.StandardButton.Yes: - self.clear_state() + self.new_session() + + def _load_from_file(self): + names = QFileDialog.getOpenFileName(self, 'Load session', self._session_path, self.FILE_REGEX_YAML) + + if names[0] == '': + return + + file_name = names[0] + with open(file_name, 'r', encoding='UTF8') as handle: + self._container.populate_from_file_yaml(handle) + + def _save_to_file(self): + """Save the current geometry samples to a file""" + names = QFileDialog.getSaveFileName(self, 'Save session', self._helper.current_folder, self.FILE_REGEX_YAML) + + if names[0] == '': + return + + self._helper.current_folder = os.path.dirname(names[0]) + + if not names[0].endswith(".yaml") and names[0].find(".") < 0: + file_name = names[0] + ".yaml" + else: + file_name = names[0] + + with open(file_name, 'w', encoding='UTF8') as handle: + self._container.save_as_yaml_file(handle) def _change_step(self, step): """Update the widget to display the new step""" @@ -256,8 +291,17 @@ def _update_ui_reading(self, is_reading: bool): is_enabled = not is_reading self._step_measure.setEnabled(is_enabled) - self._step_next_button.setEnabled(is_enabled) - self._step_previous_button.setEnabled(is_enabled) + self._step_next_button.setEnabled(is_enabled and self._current_step.has_next()) + self._step_previous_button.setEnabled(is_enabled and self._current_step.has_previous()) + + self._data_status_origin.setEnabled(is_enabled) + self._data_status_x_axis.setEnabled(is_enabled) + self._data_status_xy_plane.setEnabled(is_enabled) + self._data_status_xyz_space.setEnabled(is_enabled) + + self._load_button.setEnabled(is_enabled) + self._save_button.setEnabled(is_enabled) + self._clear_all_button.setEnabled(is_enabled) def _update_solution_info(self): solution = self._latest_solution @@ -312,11 +356,14 @@ def _notify_user(self, notification_type: _UserNotificationType): case _UserNotificationType.SUCCESS: self._helper.cf.platform.send_user_notification(True) self._sample_collection_box.setStyleSheet(STYLE_GREEN_BACKGROUND) + self._update_ui_reading(False) case _UserNotificationType.FAILURE: self._helper.cf.platform.send_user_notification(False) self._sample_collection_box.setStyleSheet(STYLE_RED_BACKGROUND) + self._update_ui_reading(False) case _UserNotificationType.PENDING: self._sample_collection_box.setStyleSheet(STYLE_YELLOW_BACKGROUND) + self._update_ui_reading(True) self._user_notification_clear_timer.stop() self._user_notification_clear_timer.start(1000)