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..17068a11 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 @@ -41,11 +42,14 @@ 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 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 @@ -260,6 +264,11 @@ 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""" @@ -295,6 +304,9 @@ 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) @@ -355,10 +367,15 @@ 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() - def write_and_store_geometry(self, geometries): + def write_and_store_geometry(self, geometries: dict[int, LighthouseBsGeometry]): + # 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, geos=geometries) @@ -386,6 +403,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': @@ -481,6 +499,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() @@ -515,6 +534,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, @@ -532,6 +559,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._geo_estimator_widget.setVisible(self._ui_mode == UiMode.geo_estimation and enabled) + 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..fa9a0687 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 - - + + + + + + + Position: + + + + + + + + 150 + 0 + + + + QFrame::NoFrame + + + (0.0 , 0.0 , 0.0) + + + - - - - - - - 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 - - - - - - - - - - - - - - - - - - + + + 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 + + + - - - - - - - Manage geometry - - - - - - - Change system type - - - - - - - Set BS channel - - - - - - - - - - - Save system config - - - - - - - Load system config - - - - - - - - - Qt::Vertical - - - - 20 - 5 - - - - - + + + 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,21 @@ - - - - QLayout::SetMaximumSize + + + + + + + Qt::Vertical - + + + 20 + 40 + + + diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui new file mode 100644 index 00000000..5c8ebdfe --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -0,0 +1,310 @@ + + + Form + + + + 0 + 0 + 400 + 753 + + + + Form + + + + + + + true + + + + Geometry estimator + + + + + + + + + + Sample collection + + + + + + TextLabel + + + + + + + + + + Image + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + + 0 + 0 + + + + Start measurement + + + + + + + QFrame::Panel + + + TextLabel + + + + + + + Qt::Horizontal + + + + + + + + + + 0 + 0 + + + + Previous + + + + + + + + 0 + 0 + + + + Next + + + + + + + + + + + + + + + 0 + 0 + + + + Data status + + + + + + + 0 + 0 + + + + + + + Origin + + + false + + + + + + + + + + X-axis + + + false + + + + + + + XY-plane + + + false + + + + + + + XYZ-space + + + false + + + + + + + + + + + 0 + 0 + + + + Solution status + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + + + + + + Base station links + + + + + + qwe + + + + + + + + + + + Session management + + + + + + Load + + + + + + + Save copy as... + + + + + + + New session + + + + + + + + + + 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 00000000..0b1d1f75 Binary files /dev/null and b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_1.png differ 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 00000000..56dfb5fb Binary files /dev/null and b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_2.png differ 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 00000000..e5f385e4 Binary files /dev/null and b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_3.png differ diff --git a/src/cfclient/ui/widgets/geo_estimator_resources/bslh_4.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_4.png new file mode 100644 index 00000000..434ff48e Binary files /dev/null and b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_4.png differ diff --git a/src/cfclient/ui/widgets/geo_estimator_resources/bslh_5.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_5.png new file mode 100644 index 00000000..c20847a4 Binary files /dev/null and b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_5.png differ diff --git a/src/cfclient/ui/widgets/geo_estimator_widget.py b/src/cfclient/ui/widgets/geo_estimator_widget.py new file mode 100644 index 00000000..bc798cc8 --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -0,0 +1,542 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2025 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. + +# You should have received a copy of the GNU General Public License along with +# this program; if not, write to the Free Software Foundation, Inc., +# 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +""" +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 + + +import logging +from enum import Enum +import threading + +import cfclient + +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 LighthouseMatchedSweepAngleReader +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors +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 + +__author__ = 'Bitcraze AB' +__all__ = ['GeoEstimatorWidget'] + +logger = logging.getLogger(__name__) + +(geo_estimator_widget_class, connect_widget_base_class) = ( + uic.loadUiType(cfclient.module_path + '/ui/widgets/geo_estimator.ui')) + + +REFERENCE_DIST = 1.0 + + +class _CollectionStep(Enum): + ORIGIN = ('bslh_1.png', + 'Step 1. Origin', + 'Put the Crazyflie where you want the origin of your coordinate system.\n') + X_AXIS = ('bslh_2.png', + 'Step 2. X-axis', + '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.') + XY_PLANE = ('bslh_3.png', + 'Step 3. XY-plane', + '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.') + XYZ_SPACE = ('bslh_4.png', + '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' + + 'left-right around the Z-axis and then holding it still for a second.\n') + + def __init__(self, image, title, instructions): + self.image = image + self.title = title + self.instructions = instructions + + self._order = None + + @property + def order(self): + """Get the order of the steps in the collection process""" + if self._order is None: + self._order = [self.ORIGIN, + self.X_AXIS, + self.XY_PLANE, + self.XYZ_SPACE] + return self._order + + def next(self): + """Get the next step in the collection process""" + for i, step in enumerate(self.order): + if step == self: + if i + 1 < len(self.order): + return self.order[i + 1] + else: + return self + + def has_next(self): + """Check if there is a next step in the collection process""" + return self.next() != self + + def previous(self): + """Get the previous step in the collection process""" + for i, step in enumerate(self.order): + if step == self: + if i - 1 >= 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 _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): + """Widget for the geometry estimator UI""" + + _timeout_reader_signal = QtCore.pyqtSignal(object) + _container_updated_signal = QtCore.pyqtSignal() + _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) + + self._lighthouse_tab = lighthouse_tab + self._helper = lighthouse_tab._helper + + 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) + 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) + self._timeout_reader_result_setter = None + + 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, + 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() + + self._current_step = _CollectionStep.ORIGIN + 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 + + 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: + 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: + self._action_detector.stop() + if self._solver_thread is not None: + logger.info("Stopping solver thread") + self._solver_thread.stop(do_join=False) + self._solver_thread = None + + def new_session(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.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""" + if step != self._current_step: + self._current_step = step + self._update_step_ui() + if step == _CollectionStep.XYZ_SPACE: + self._action_detector.start() + else: + self._action_detector.stop() + + def _update_step_ui(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('') + + 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()) + + 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 + + self._step_measure.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 + + 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: + 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: + 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) + + 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) + + 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 _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) + 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) + + 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: + 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: + 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: + self._measure_xyz_space() + + def _measure_origin(self): + """Measure the 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""" + 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""" + 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() + 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.") + 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 + + def _solution_ready_cb(self, solution: LighthouseGeometrySolution): + 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) + + 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) + + def _user_action_detected_cb(self): + self._measure_xyz_space() + + def _single_sample_ready_cb(self, sample: LhCfPoseSample): + self._user_notification_signal.emit(_UserNotificationType.SUCCESS) + self._container_updated_signal.emit() + self._container.append_xyz_space_samples([sample]) + + def _single_sample_timeout_cb(self): + self._user_notification_signal.emit(_UserNotificationType.FAILURE) + + +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 + + # Can not stop the timer from this thread, let it run. + # 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) 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..c99543f4 100644 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py @@ -30,21 +30,23 @@ from __future__ import annotations import cfclient +import logging 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_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, LhCfPoseSample +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 -import time + +logger = logging.getLogger(__name__) REFERENCE_DIST = 1.0 @@ -59,17 +61,17 @@ 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.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) - - def _finish_button_clicked_callback(self): - self.ready_cb(self.get_geometry_page.get_geometry()) + logger.info("Wizard started") def reset(self): self.setWindowFlags(self.windowFlags() & ~QtCore.Qt.WindowType.WindowContextHelpButtonHint) @@ -80,35 +82,61 @@ 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, 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}') + + # Upload the geometry to the Crazyflie + geo_dict = {} + for bs_id, pose in solution.poses.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) + + def showEvent(self, event): + self.solver_thread.start() + + def closeEvent(self, event): + self.solver_thread.stop() + 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() @@ -173,12 +201,10 @@ 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(): angles_calibrated[bs_id] = data[1] - self.recorded_angle_result = LhCfPoseSample(angles_calibrated=angles_calibrated) self.visible_basestations = ', '.join(map(lambda x: str(x + 1), recorded_angles.keys())) amount_of_basestations = len(recorded_angles.keys()) @@ -195,6 +221,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: @@ -211,6 +238,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 @@ -225,18 +255,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.') @@ -244,10 +278,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.') @@ -255,211 +293,48 @@ 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) - 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') + def __init__(self, cf: Crazyflie, container: LhGeoInputContainer, parent=None): + super(RecordXYZSpaceSamplesPage, self).__init__(cf, container) + 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) - self.bs_seen = set() - - 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 _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 user_action_cb(self): + self.reader.start() + + 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() -class EstimateGeometryThread(QtCore.QObject): - finished = QtCore.pyqtSignal() - failed = QtCore.pyqtSignal() - - def __init__(self, origin, x_axis, xy_plane, samples): - super(EstimateGeometryThread, self).__init__() - - self.origin = origin - self.x_axis = x_axis - self.xy_plane = xy_plane - self.samples = samples - self.bs_poses = {} - - def run(self): - try: - self.bs_poses = self._estimate_geometry(self.origin, self.x_axis, self.xy_plane, self.samples) - self.finished.emit() - except Exception as ex: - print(ex) - self.failed.emit() - - 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]: - """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) - - solution = LighthouseGeometrySolver.solve(initial_guess, - cleaned_matched_samples, - LhDeck4SensorPositions.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) - origin_pos = solution.cf_poses[0].translation - x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(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_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 - - -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...')) - 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() - self.thread_estimator = QtCore.QThread() - self.worker = EstimateGeometryThread(origin, x_axis, xy_plane, samples) - 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()) + def cleanupPage(self): + self._stop_all() + super().cleanupPage()