diff --git a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py deleted file mode 100644 index f8658bdc7..000000000 --- a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py +++ /dev/null @@ -1,232 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2021-2023 Bitcraze AB -# -# 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. - -""" -Dialog box used to configure base station geometry. Used from the lighthouse tab. -""" -import logging - -import cfclient -from PyQt6 import QtWidgets -from PyQt6 import uic -from PyQt6.QtCore import QVariant, Qt, QAbstractTableModel, pyqtSignal -from cflib.localization import LighthouseBsGeoEstimator -from cflib.localization import LighthouseSweepAngleAverageReader -from cflib.crazyflie.mem import LighthouseBsGeometry -from cfclient.ui.wizards.lighthouse_geo_bs_estimation_wizard import LighthouseBasestationGeometryWizard - -__author__ = 'Bitcraze AB' -__all__ = ['LighthouseBsGeometryDialog'] - -logger = logging.getLogger(__name__) - -(basestation_geometry_widget_class, connect_widget_base_class) = ( - uic.loadUiType( - cfclient.module_path + '/ui/dialogs/lighthouse_bs_geometry_dialog.ui') -) - - -class LighthouseBsGeometryTableModel(QAbstractTableModel): - def __init__(self, headers, parent=None, *args): - QAbstractTableModel.__init__(self, parent) - self._headers = headers - self._table_values = [] - self._current_geos = {} - self._estimated_geos = {} - - def rowCount(self, parent=None, *args, **kwargs): - return len(self._table_values) - - def columnCount(self, parent=None, *args, **kwargs): - return len(self._headers) - - def data(self, index, role=None): - if index.isValid(): - value = self._table_values[index.row()][index.column()] - if role == Qt.ItemDataRole.DisplayRole: - return QVariant(value) - - return QVariant() - - def headerData(self, col, orientation, role=None): - if orientation == Qt.Orientation.Horizontal and role == Qt.ItemDataRole.DisplayRole: - return QVariant(self._headers[col]) - return QVariant() - - def _compile_entry(self, current_geo, estimated_geo, index): - result = 'N/A' - if current_geo is not None: - result = '%.2f' % current_geo.origin[index] - if estimated_geo is not None: - result += ' -> %.2f' % estimated_geo.origin[index] - - return result - - def _add_table_value(self, current_geo, estimated_geo, id, table_values): - x = self._compile_entry(current_geo, estimated_geo, 0) - y = self._compile_entry(current_geo, estimated_geo, 1) - z = self._compile_entry(current_geo, estimated_geo, 2) - - table_values.append([id + 1, x, y, z]) - - def _add_table_value_for_id(self, current_geos, estimated_geos, table_values, id): - current_geo = None - if id in current_geos: - current_geo = current_geos[id] - - estimated_geo = None - if id in estimated_geos: - estimated_geo = estimated_geos[id] - - if current_geo is not None or estimated_geo is not None: - self._add_table_value(current_geo, estimated_geo, id, table_values) - - def _add_table_values(self, current_geos, estimated_geos, table_values): - current_ids = set(current_geos.keys()) - estimated_ids = set(estimated_geos.keys()) - all_ids = current_ids.union(estimated_ids) - - for id in all_ids: - self._add_table_value_for_id(current_geos, estimated_geos, table_values, id) - - def _update_table_data(self): - self.layoutAboutToBeChanged.emit() - self._table_values = [] - self._add_table_values(self._current_geos, self._estimated_geos, self._table_values) - self._table_values.sort(key=lambda row: row[0]) - self.layoutChanged.emit() - - def set_estimated_geos(self, geos): - self._estimated_geos = geos - self._update_table_data() - - def set_current_geos(self, geos): - self._current_geos = geos - self._update_table_data() - - -class LighthouseBsGeometryDialog(QtWidgets.QWidget, basestation_geometry_widget_class): - - _sweep_angles_received_and_averaged_signal = pyqtSignal(object) - _base_station_geometery_received_signal = pyqtSignal(object) - - def __init__(self, lighthouse_tab, *args): - super(LighthouseBsGeometryDialog, self).__init__(*args) - self.setupUi(self) - - self._lighthouse_tab = lighthouse_tab - - self._estimate_geometry_button.clicked.connect(self._estimate_geometry_button_clicked) - self._simple_estimator = LighthouseBsGeoEstimator() - self._estimate_geometry_simple_button.clicked.connect(self._estimate_geometry_simple_button_clicked) - try: - if not self._simple_estimator.is_available(): - self._estimate_geometry_simple_button.setEnabled(False) - except Exception as e: - print(e) - - self._write_to_cf_button.clicked.connect(self._write_to_cf_button_clicked) - - self._sweep_angles_received_and_averaged_signal.connect(self._sweep_angles_received_and_averaged_cb) - self._base_station_geometery_received_signal.connect(self._basestation_geometry_received_signal_cb) - self._close_button.clicked.connect(self.close) - - self._sweep_angle_reader = LighthouseSweepAngleAverageReader( - 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._lh_geos = None - self._newly_estimated_geometry = {} - - # Table handlers - self._headers = ['id', 'x', 'y', 'z'] - self._data_model = LighthouseBsGeometryTableModel(self._headers, self) - self._table_view.setModel(self._data_model) - - self._table_view.verticalHeader().setVisible(False) - - header = self._table_view.horizontalHeader() - header.setSectionResizeMode(0, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) - header.setSectionResizeMode(1, QtWidgets.QHeaderView.ResizeMode.Stretch) - header.setSectionResizeMode(2, QtWidgets.QHeaderView.ResizeMode.Stretch) - header.setSectionResizeMode(3, QtWidgets.QHeaderView.ResizeMode.Stretch) - - self._update_ui() - - def reset(self): - self._newly_estimated_geometry = {} - self._update_ui() - - def _basestation_geometry_received_signal_cb(self, basestation_geometries): - self._newly_estimated_geometry = basestation_geometries - self.show() - self._update_ui() - - def _sweep_angles_received_and_averaged_cb(self, averaged_angles): - self._averaged_angles = averaged_angles - self._newly_estimated_geometry = {} - - for id, average_data in averaged_angles.items(): - sensor_data = average_data[1] - rotation_bs_matrix, position_bs_vector = self._simple_estimator.estimate_geometry(sensor_data) - geo = LighthouseBsGeometry() - geo.rotation_matrix = rotation_bs_matrix - geo.origin = position_bs_vector - geo.valid = True - self._newly_estimated_geometry[id] = geo - - self._update_ui() - - def _estimate_geometry_button_clicked(self): - self._base_station_geometry_wizard.reset() - self._base_station_geometry_wizard.show() - self.hide() - - def _estimate_geometry_simple_button_clicked(self): - self._sweep_angle_reader.start_angle_collection() - self._update_ui() - - def _write_to_cf_button_clicked(self): - if len(self._newly_estimated_geometry) > 0: - self._lighthouse_tab.write_and_store_geometry(self._newly_estimated_geometry) - self._newly_estimated_geometry = {} - - self._update_ui() - - def _update_ui(self): - self._write_to_cf_button.setEnabled(len(self._newly_estimated_geometry) > 0) - self._data_model.set_estimated_geos(self._newly_estimated_geometry) - - def closeEvent(self, event): - self._stop_collection() - - def _stop_collection(self): - self._sweep_angle_reader.stop_angle_collection() - - def geometry_updated(self, geometry): - self._data_model.set_current_geos(geometry) - - self._update_ui() diff --git a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.ui b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.ui deleted file mode 100644 index 86fb1e6d5..000000000 --- a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.ui +++ /dev/null @@ -1,117 +0,0 @@ - - - Form - - - Qt::ApplicationModal - - - - 0 - 0 - 443 - 555 - - - - Basestation Geometry Managment - - - - - - - - - - Estimate Geometry - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Estimate Geometry Simple - - - - - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::Horizontal - - - - - - - - - Write to Crazyflie - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Close - - - - - - - - - - - - diff --git a/src/cfclient/ui/pose_logger.py b/src/cfclient/ui/pose_logger.py index 2f3455037..5105890d7 100644 --- a/src/cfclient/ui/pose_logger.py +++ b/src/cfclient/ui/pose_logger.py @@ -34,6 +34,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.utils.callbacks import Caller +from cflib.localization import Pose __author__ = 'Bitcraze AB' __all__ = ['PoseLogger'] @@ -78,6 +79,11 @@ def rpy_rad(self): """Get the roll, pitch and yaw of the full pose in radians""" return [math.radians(self.pose[3]), math.radians(self.pose[4]), math.radians(self.pose[5])] + @property + def full_pose(self) -> Pose: + """Get the full pose as a Pose object""" + return Pose.from_cf_rpy(roll=self.pose[3], pitch=self.pose[4], yaw=self.pose[5], t_vec=self.position) + def _connected(self, link_uri) -> None: logConf = LogConfig("Pose", 40) logConf.add_variable(self.LOG_NAME_ESTIMATE_X, "float") diff --git a/src/cfclient/ui/tabs/lighthouse_tab.py b/src/cfclient/ui/tabs/lighthouse_tab.py index 8d2969942..5bac8a501 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.py +++ b/src/cfclient/ui/tabs/lighthouse_tab.py @@ -31,29 +31,35 @@ """ import logging +from enum import Enum from PyQt6 import uic from PyQt6.QtCore import Qt, pyqtSignal, QTimer -from PyQt6.QtWidgets import QMessageBox -from PyQt6.QtWidgets import QFileDialog -from PyQt6.QtWidgets import QLabel +from PyQt6.QtWidgets import QMessageBox, QFileDialog, QLabel, QPushButton +from vispy.util.event import Event import cfclient from cfclient.ui.tab_toolbox import TabToolbox +from cfclient.ui.widgets.geo_estimator_widget import GeoEstimatorWidget +from cfclient.ui.widgets.geo_estimator_details_widget import GeoEstimatorDetailsWidget +from cfclient.ui.widgets.info_label import InfoLabel from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import LighthouseMemHelper from cflib.localization import LighthouseConfigWriter from cflib.localization import LighthouseConfigFileManager +from cflib.localization import LighthouseGeometrySolution +from cflib.localization import LhCfPoseSampleType +from cflib.localization import Pose + +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 from cfclient.utils.logconfigreader import FILE_REGEX_YAML from vispy import scene import numpy as np -import math import os __author__ = 'Bitcraze AB' @@ -80,30 +86,27 @@ class MarkerPose(): LABEL_SIZE = 100 LABEL_OFFSET = np.array((0.0, 0, 0.25)) - def __init__(self, the_scene, color, text=None): + def __init__(self, the_scene, color, text=None, axis_visible: bool = False, interactive=False, + symbol: str = 'disc'): self._scene = the_scene self._color = color self._text = text + self._position = [0.0, 0, 0] + self._rot = np.identity(3) + self._symbol = symbol + self._axis_visible = False + self._x_axis = None + self._y_axis = None + self._z_axis = None self._marker = scene.visuals.Markers( pos=np.array([[0, 0, 0]]), parent=self._scene, - face_color=self._color) - - self._x_axis = scene.visuals.Line( - pos=np.array([[0, 0, 0], [0, 0, 0]]), - color=self.COL_X_AXIS, - parent=self._scene) - - self._y_axis = scene.visuals.Line(pos=np.array( - [[0, 0, 0], [0, 0, 0]]), - color=self.COL_Y_AXIS, - parent=self._scene) + face_color=self._color, + symbol=self._symbol) - self._z_axis = scene.visuals.Line( - pos=np.array([[0, 0, 0], [0, 0, 0]]), - color=self.COL_Z_AXIS, - parent=self._scene) + if interactive: + self._marker.interactive = True self._label = None if self._text: @@ -113,39 +116,215 @@ def __init__(self, the_scene, color, text=None): pos=self.LABEL_OFFSET, parent=self._scene) + self.set_axis_visible(axis_visible) + + def set_axis_visible(self, visible: bool): + if visible == self._axis_visible: + return + + if visible: + if self._x_axis is None: + self._x_axis = scene.visuals.Line( + pos=np.array([[0, 0, 0], [0, 0, 0]]), + color=self.COL_X_AXIS, + parent=self._scene) + + if self._y_axis is None: + self._y_axis = scene.visuals.Line( + pos=np.array([[0, 0, 0], [0, 0, 0]]), + color=self.COL_Y_AXIS, + parent=self._scene) + + if self._z_axis is None: + self._z_axis = scene.visuals.Line( + pos=np.array([[0, 0, 0], [0, 0, 0]]), + color=self.COL_Z_AXIS, + parent=self._scene) + else: + if self._x_axis is not None: + self._x_axis.parent = None + self._x_axis = None + if self._y_axis is not None: + self._y_axis.parent = None + self._y_axis = None + if self._z_axis is not None: + self._z_axis.parent = None + self._z_axis = None + + self._axis_visible = visible + + self._update_visuals() + def set_pose(self, position, rot): - self._marker.set_data(pos=np.array([position]), face_color=self._color) + if np.array_equal(position, self._position) and np.array_equal(rot, self._rot): + return - if self._label: - self._label.pos = self.LABEL_OFFSET + position + self._position = position + self._rot = rot - x_tip = np.dot(np.array(rot), np.array([self.AXIS_LEN, 0, 0])) - self._x_axis.set_data(np.array([position, x_tip + position]), color=self.COL_X_AXIS) + self._update_visuals() - y_tip = np.dot(np.array(rot), np.array([0, self.AXIS_LEN, 0])) - self._y_axis.set_data(np.array([position, y_tip + position]), color=self.COL_Y_AXIS) + def _update_visuals(self): + self._marker.set_data(pos=np.array([self._position]), face_color=self._color, symbol=self._symbol) - z_tip = np.dot(np.array(rot), np.array([0, 0, self.AXIS_LEN])) - self._z_axis.set_data(np.array([position, z_tip + position]), color=self.COL_Z_AXIS) + if self._label: + self._label.pos = self.LABEL_OFFSET + self._position + + if self._axis_visible: + x_tip = np.dot(np.array(self._rot), np.array([self.AXIS_LEN, 0, 0])) + self._x_axis.set_data(np.array([self._position, x_tip + self._position]), color=self.COL_X_AXIS) + y_tip = np.dot(np.array(self._rot), np.array([0, self.AXIS_LEN, 0])) + self._y_axis.set_data(np.array([self._position, y_tip + self._position]), color=self.COL_Y_AXIS) + z_tip = np.dot(np.array(self._rot), np.array([0, 0, self.AXIS_LEN])) + self._z_axis.set_data(np.array([self._position, z_tip + self._position]), color=self.COL_Z_AXIS) + + def get_position(self): + return self._position def remove(self): self._marker.parent = None - self._x_axis.parent = None - self._y_axis.parent = None - self._z_axis.parent = None + if self._x_axis is not None: + self._x_axis.parent = None + if self._y_axis is not None: + self._y_axis.parent = None + if self._z_axis is not None: + self._z_axis.parent = None if self._label: self._label.parent = None def set_color(self, color): self._color = color - self._marker.set_data(face_color=self._color) + self._marker.set_data(pos=np.array([self._position]), face_color=self._color, symbol=self._symbol) + def is_same_visual(self, visual): + if not self._marker.interactive: + raise RuntimeError("is_same_visual can only be used for interactive markers") -class Plot3dLighthouse(scene.SceneCanvas): + return visual == self._marker + + +class CfMarkerPose(MarkerPose): POSITION_BRUSH = np.array((0, 0, 1.0)) + + def __init__(self, the_scene): + super().__init__(the_scene, self.POSITION_BRUSH, None, axis_visible=True) + + +class BsMarkerPose(MarkerPose): + HIGHLIGHT_BRUSH = np.array((0.2, 0.2, 0.5)) BS_BRUSH_VISIBLE = np.array((0.2, 0.5, 0.2)) BS_BRUSH_NOT_VISIBLE = np.array((0.8, 0.5, 0.5)) + LINE_COL = np.array((0.0, 0.0, 0.0)) + + def __init__(self, the_scene, text=None): + super().__init__(the_scene, self.BS_BRUSH_NOT_VISIBLE, text, axis_visible=True, interactive=True) + + self._is_visible = False + self._is_highlighted = False + self._bs_lines = [] + + def set_receiving_status(self, visible: bool): + self._is_visible = visible + self.set_color(self._get_brush()) + + def set_highlighted(self, highlighted: bool, other_positions=[]): + if highlighted: + for i, pos in enumerate(other_positions): + if i >= len(self._bs_lines): + line = scene.visuals.Line( + pos=np.array([[0, 0, 0], [0, 0, 0]]), + color=self.LINE_COL, + parent=self._scene) + self._bs_lines.append(line) + else: + line = self._bs_lines[i] + + line.set_data(np.array([self._position, pos]), color=self.LINE_COL) + + for _ in range(len(self._bs_lines) - len(other_positions)): + line = self._bs_lines.pop() + line.parent = None + else: + self._clear_lines() + + self._is_highlighted = highlighted + self.set_color(self._get_brush()) + def remove(self): + super().remove() + self._clear_lines() + + def _clear_lines(self): + for line in self._bs_lines: + line.parent = None + self._bs_lines = [] + + def _get_brush(self) -> np.ndarray: + if self._is_highlighted: + return self.HIGHLIGHT_BRUSH + elif self._is_visible: + return self.BS_BRUSH_VISIBLE + else: + return self.BS_BRUSH_NOT_VISIBLE + + +class SampleMarkerPose(MarkerPose): + NORMAL_BRUSH = np.array((0.8, 0.8, 0.8)) + VERIFICATION_BRUSH = np.array((1.0, 1.0, 0.9)) + HIGHLIGHT_BRUSH = np.array((0.2, 0.2, 0.2)) + BS_LINE_COL = np.array((0.0, 0.0, 0.0)) + + def __init__(self, the_scene): + super().__init__(the_scene, self.NORMAL_BRUSH, None, interactive=True, symbol='square') + self._is_highlighted = False + self._is_verification = False + self._bs_lines = [] + + def set_highlighted(self, highlighted: bool, bs_positions=[]): + if highlighted: + self.set_color(self.HIGHLIGHT_BRUSH) + + # always update lines when highlighted as base station positions may have changed + for i, pos in enumerate(bs_positions): + if i >= len(self._bs_lines): + line = scene.visuals.Line( + pos=np.array([[0, 0, 0], [0, 0, 0]]), + color=self.BS_LINE_COL, + parent=self._scene) + self._bs_lines.append(line) + else: + line = self._bs_lines[i] + + line.set_data(np.array([self._position, pos]), color=self.BS_LINE_COL) + + for _ in range(len(self._bs_lines) - len(bs_positions)): + line = self._bs_lines.pop() + line.parent = None + else: + if highlighted != self._is_highlighted: + self.set_color(self.VERIFICATION_BRUSH) if self._is_verification else self.set_color(self.NORMAL_BRUSH) + self._clear_lines() + + self.set_axis_visible(highlighted) + + self._is_highlighted = highlighted + + def set_verification_type(self, is_verification: bool): + self._is_verification = is_verification + if not self._is_highlighted: + self.set_color(self.VERIFICATION_BRUSH) if self._is_verification else self.set_color(self.NORMAL_BRUSH) + + def remove(self): + super().remove() + self._clear_lines() + + def _clear_lines(self): + for line in self._bs_lines: + line.parent = None + self._bs_lines = [] + + +class Plot3dLighthouse(scene.SceneCanvas): VICINITY_DISTANCE = 2.5 HIGHLIGHT_DISTANCE = 0.5 @@ -156,24 +335,32 @@ class Plot3dLighthouse(scene.SceneCanvas): TEXT_OFFSET = np.array((0.0, 0, 0.25)) - def __init__(self): + DEFAULT_CAMERA_DISTANCE = 10.0 + + def __init__(self, sample_clicked_signal: pyqtSignal(int), base_station_clicked_signal: pyqtSignal(int)): scene.SceneCanvas.__init__(self, keys=None) self.unfreeze() self._view = self.central_widget.add_view() self._view.bgcolor = '#ffffff' self._view.camera = scene.TurntableCamera( - distance=10.0, + distance=self.DEFAULT_CAMERA_DISTANCE, up='+z', center=(0.0, 0.0, 1.0)) + self._view.camera.set_default_state() - self._cf = None - self._base_stations = {} + self._cf: CfMarkerPose | None = None + self._base_stations: dict[int, BsMarkerPose] = {} + self._samples: list[SampleMarkerPose] = [] + self.selected_sample_index: int = -1 + self.selected_base_station_id: int = -1 - self.freeze() + self.events.mouse_press.connect(self.on_mouse_press) + self._sample_clicked_signal = sample_clicked_signal + self._base_station_clicked_signal = base_station_clicked_signal plane_size = 10 - scene.visuals.Plane( + self._plane = scene.visuals.Plane( width=plane_size, height=plane_size, width_segments=plane_size, @@ -181,9 +368,55 @@ def __init__(self): color=(0.5, 0.5, 0.5, 0.5), edge_color="gray", parent=self._view.scene) + self._plane.interactive = True self._addArrows(1, 0.02, 0.1, 0.1, self._view.scene) + self._home_button = QPushButton('Home', self.native) + self._home_button.clicked.connect(self.move_camera_home) + + self.freeze() + + def move_camera_home(self): + self._view.camera.reset() + self._view.camera.distance = self.DEFAULT_CAMERA_DISTANCE + + def on_mouse_press(self, event): + visual = self.visual_at(event.pos) + + is_object_hit = False + + # Check if the plane was hit. This will prevent deselecting samples and base stations when rotating the camera + # with the mouse over the plane. + if visual == self._plane: + is_object_hit = True + + if not is_object_hit: + for index, sample in enumerate(self._samples): + if sample.is_same_visual(visual): + clicked_index = index + self._sample_clicked_signal.emit(clicked_index) + is_object_hit = True + break + + if not is_object_hit: + for id, bs in self._base_stations.items(): + if bs.is_same_visual(visual): + is_object_hit = True + self._base_station_clicked_signal.emit(id) + break + + if not is_object_hit: + self._sample_clicked_signal.emit(-1) + self._base_station_clicked_signal.emit(-1) + + def on_resize(self, event: Event): + x = self.native.width() - self._home_button.width() - 5 + y = 5 + self._home_button.move(x, y) + + return super().on_resize(event) + def _addArrows(self, length, width, head_length, head_width, parent): # The Arrow visual in vispy does not seem to work very good, # draw arrows using lines instead. @@ -224,28 +457,95 @@ def _addArrows(self, length, width, head_length, head_width, parent): [0, -w, 0]], width=1.0, color='blue', parent=parent, marker_size=0.0) - def update_cf_pose(self, position, rot): + def update_cf_pose(self, pose: Pose): if not self._cf: - self._cf = MarkerPose(self._view.scene, self.POSITION_BRUSH) - self._cf.set_pose(position, rot) + self._cf = CfMarkerPose(self._view.scene) + self._cf.set_pose(pose.translation, pose.rot_matrix) + + # if self._follow_drone: + # self._update_cam_to_follow_drone_view(pose) + + def _update_cam_to_follow_drone_view(self, pose: Pose): + # Unfortunately this does not fully work as intended yet, not sure why. + self._view.camera.center = pose.translation + + elevation, azimuth, roll = self._rotation_to_camera_parameters(pose) + self._view.camera.elevation = elevation + self._view.camera.azimuth = azimuth + self._view.camera.roll = roll + + def _rotation_to_camera_parameters(self, pose: Pose): + # This conversion is the inverse of _get_rotation_tr() in turntable.py (in vispy). It has been verified using + # _get_rotation_tr() and seems to work correctly. + angles = pose.rot_euler(seq='XZY', degrees=True) + if abs(angles[0]) < 90: + elevation = angles[0] + azimuth = -angles[1] + roll = -angles[2] + else: + elevation = 180 + angles[0] + if elevation > 180: + elevation -= 360 + azimuth = 180 + angles[1] + if azimuth > 180: + azimuth -= 360 + roll = 180 - angles[2] + if roll > 180: + roll -= 360 + + return elevation, azimuth, roll + + def update_base_station_geos(self, geos, solution: LighthouseGeometrySolution): + # Geos are read from the CF (not the solution) to reflect the actual stored data + # The solution is only used to get link information (if available) for highlighting - def update_base_station_geos(self, geos): for id, geo in geos.items(): + # Add a new base station if it does not exist if (geo is not None) and (id not in self._base_stations): - self._base_stations[id] = MarkerPose(self._view.scene, self.BS_BRUSH_NOT_VISIBLE, text=f"{id + 1}") + self._base_stations[id] = BsMarkerPose(self._view.scene, text=f"{id + 1}") + self._base_stations[id].set_pose(geo.origin, geo.rotation_matrix) + # Highlight if selected + if id == self.selected_base_station_id: + # We have two options of what to show, either the positions of other base stations or + # the positions of samples that have measurements from this base station. + other_positions = self._get_positions_of_linked_base_stations(id, solution, geos) + # other_positions = self._get_positions_of_samples_with_bs(id, solution) + + self._base_stations[id].set_highlighted(True, other_positions=other_positions) + else: + self._base_stations[id].set_highlighted(False) + + # Remove any base stations that are no longer present geos_to_remove = self._base_stations.keys() - geos.keys() for id in geos_to_remove: existing = self._base_stations.pop(id) existing.remove() + def _get_positions_of_linked_base_stations(self, bs_id: int, solution: LighthouseGeometrySolution, + geos: dict[int, LighthouseBsGeometry]) -> list[float]: + linked_base_stations = solution.link_count[bs_id].keys() + positions = [] + for other_id in linked_base_stations: + if other_id in geos: + geo = geos[other_id] + if geo is not None: + positions.append(geo.origin) + return positions + + def _get_positions_of_samples_with_bs(self, bs_id: int, solution: LighthouseGeometrySolution) -> list[float]: + positions = [] + for sample in solution.samples: + if sample.sample_type != LhCfPoseSampleType.VERIFICATION: + if bs_id in sample.base_station_ids: + pose = sample.pose + positions.append(pose.translation) + return positions + def update_base_station_visibility(self, visibility): for id, bs in self._base_stations.items(): - if id in visibility: - bs.set_color(self.BS_BRUSH_VISIBLE) - else: - bs.set_color(self.BS_BRUSH_NOT_VISIBLE) + bs.set_receiving_status(id in visibility) def clear(self): if self._cf: @@ -255,9 +555,44 @@ def clear(self): for bs in self._base_stations.values(): bs.remove() self._base_stations = {} + self.clear_samples() + + def update_samples(self, solution: LighthouseGeometrySolution): + marker_idx = 0 + for smpl_idx, pose_smpl in enumerate(solution.samples): + if pose_smpl.has_pose: + pose = pose_smpl.pose + if marker_idx >= len(self._samples): + self._samples.append(SampleMarkerPose(self._view.scene)) - def _mix(self, col1, col2, mix): - return col1 * mix + col2 * (1.0 - mix) + self._samples[marker_idx].set_pose(pose.translation, pose.rot_matrix) + self._samples[marker_idx].set_verification_type( + pose_smpl.sample_type == LhCfPoseSampleType.VERIFICATION) + + if smpl_idx == self.selected_sample_index: + bs_positions = [] + for id in pose_smpl.base_station_ids: + if id in self._base_stations: + bs_positions.append(self._base_stations[id].get_position()) + self._samples[marker_idx].set_highlighted(True, bs_positions=bs_positions) + else: + self._samples[marker_idx].set_highlighted(False) + + marker_idx += 1 + + for sample in self._samples[marker_idx:]: + sample.remove() + del self._samples[marker_idx:] + + def clear_samples(self): + for sample in self._samples: + sample.remove() + self._samples = [] + + +class UiMode(Enum): + flying = 1 + geo_estimation = 2 class LighthouseTab(TabToolbox, lighthouse_tab_class): @@ -273,7 +608,6 @@ class LighthouseTab(TabToolbox, lighthouse_tab_class): STATUS_MISSING_DATA = 1 STATUS_TO_ESTIMATOR = 2 - # TODO change these names to something more logical LOG_STATUS = "lighthouse.status" LOG_RECEIVE = "lighthouse.bsReceive" LOG_CALIBRATION_EXISTS = "lighthouse.bsCalVal" @@ -290,11 +624,37 @@ class LighthouseTab(TabToolbox, lighthouse_tab_class): _new_system_config_written_to_cf_signal = pyqtSignal(bool) _geometry_read_signal = pyqtSignal(object) _calibration_read_signal = pyqtSignal(object) + _sample_clicked_signal = pyqtSignal(int) + _base_station_clicked_signal = pyqtSignal(int) def __init__(self, helper): super(LighthouseTab, self).__init__(helper, 'Lighthouse Positioning') self.setupUi(self) + # Add the geometry estimator widget + self._geo_estimator_widget = GeoEstimatorWidget(self) + self._geometry_area.addWidget(self._geo_estimator_widget) + self._geo_estimator_widget.solution_ready_signal.connect(self._solution_updated_cb) + self._connected_signal.connect(self._geo_estimator_widget.cf_connected_cb) + self._geometry_read_signal.connect(self._geo_estimator_widget.geometry_has_been_read_back_cb) + + # Add the geometry estimator details widget + self._geo_estimator_details_widget = GeoEstimatorDetailsWidget() + self._details_area.addWidget(self._geo_estimator_details_widget) + self._geo_estimator_details_widget.sample_selection_changed_signal.connect(self._sample_selection_changed_cb) + self._geo_estimator_details_widget.base_station_selection_changed_signal.connect( + self._base_station_selection_changed_cb) + + # Connect signals between the geo estimator widget and the details widget + self._geo_estimator_widget.solution_ready_signal.connect(self._geo_estimator_details_widget.solution_ready_cb) + self._geo_estimator_widget._details_checkbox.stateChanged.connect( + self._geo_estimator_details_widget.details_checkbox_state_changed) + self._geo_estimator_details_widget.do_remove_sample_signal.connect(self._geo_estimator_widget.remove_sample) + self._geo_estimator_details_widget.do_convert_to_xyz_space_sample_signal.connect( + self._geo_estimator_widget.convert_to_xyz_space_sample) + self._geo_estimator_details_widget.do_convert_to_verification_sample_signal.connect( + self._geo_estimator_widget.convert_to_verification_sample) + # Always wrap callbacks from Crazyflie API though QT Signal/Slots # to avoid manipulating the UI when rendering it self._connected_signal.connect(self._connected) @@ -304,6 +664,8 @@ def __init__(self, helper): self._new_system_config_written_to_cf_signal.connect(self._new_system_config_written_to_cf) self._geometry_read_signal.connect(self._geometry_read_cb) self._calibration_read_signal.connect(self._calibration_read_cb) + self._sample_clicked_signal.connect(self._geo_estimator_details_widget.set_selected_sample) + self._base_station_clicked_signal.connect(self._geo_estimator_details_widget.set_selected_base_station) # Connect the Crazyflie API callbacks to the signals self._helper.cf.connected.add_callback(self._connected_signal.emit) @@ -344,48 +706,59 @@ def __init__(self, helper): self._graph_timer.timeout.connect(self._update_graphics) self._graph_timer.start() - self._basestation_geometry_dialog = LighthouseBsGeometryDialog(self) self._basestation_mode_dialog = LighthouseBsModeDialog(self) self._system_type_dialog = LighthouseSystemTypeDialog(helper) - self._manage_estimate_geometry_button.clicked.connect(self._show_basestation_geometry_dialog) self._change_system_type_button.clicked.connect(lambda: self._system_type_dialog.show()) self._manage_basestation_mode_button.clicked.connect(self._show_basestation_mode_dialog) - 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._latest_solution = LighthouseGeometrySolution([]) self._is_connected = False self._update_ui() - def write_and_store_geometry(self, geometries): + self._pending_geo_update = None + + self._base_stations_info_label = InfoLabel("This is information about base station status. TODO update", + self._base_station_group_box) + self._sys_management_info_label = InfoLabel("This is information about system management. TODO update", + self._sys_management_group_box) + + def write_and_store_geometry(self, geometries: dict[int, LighthouseBsGeometry]): if self._lh_config_writer: - self._lh_config_writer.write_and_store_config(self._new_system_config_written_to_cf_signal.emit, - geos=geometries) + if self._lh_config_writer.is_write_ongoing: + self._pending_geo_update = geometries + else: + self._lh_config_writer.write_and_store_config(self._new_system_config_written_to_cf_signal.emit, + geos=geometries) def _new_system_config_written_to_cf(self, success): - # Reset the bit fields for calibration data status to get a fresh view - self._helper.cf.param.set_value("lighthouse.bsCalibReset", '1') - # New geo data has been written and stored in the CF, read it back to update the UI - self._start_read_of_geo_data() - - def _show_basestation_geometry_dialog(self): - self._basestation_geometry_dialog.reset() - self._basestation_geometry_dialog.show() + if self._pending_geo_update: + # If there is a pending update, write it now + self.write_and_store_geometry(self._pending_geo_update) + self._pending_geo_update = None + else: + # Reset the bit fields for calibration data status to get a fresh view + self._helper.cf.param.set_value("lighthouse.bsCalibReset", '1') + # New geo data has been written and stored in the CF, read it back to update the UI + self._start_read_of_geo_data() def _show_basestation_mode_dialog(self): self._basestation_mode_dialog.reset() self._basestation_mode_dialog.show() def _set_up_plots(self): - self._plot_3d = Plot3dLighthouse() + self._plot_3d = Plot3dLighthouse(self._sample_clicked_signal, self._base_station_clicked_signal) self._plot_layout.addWidget(self._plot_3d.native) def _connected(self, link_uri): """Callback when the Crazyflie has been connected""" 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': @@ -426,9 +799,19 @@ def _start_read_of_geo_data(self): def _geometry_read_cb(self, geometries): # Remove any geo data where the valid flag is False self._lh_geos = dict(filter(lambda key_value: key_value[1].valid, geometries.items())) - self._basestation_geometry_dialog.geometry_updated(self._lh_geos) self._is_geometry_read_ongoing = False + def _solution_updated_cb(self, solution: LighthouseGeometrySolution): + self._latest_solution = solution + + def _sample_selection_changed_cb(self, sample_index: int): + """Callback when the sample selection in the geo estimator widget changes""" + self._plot_3d.selected_sample_index = sample_index + + def _base_station_selection_changed_cb(self, bs_id: int): + """Callback when the base station selection in the geo estimator widget changes""" + self._plot_3d.selected_base_station_id = bs_id + def _is_matching_current_geo_data(self, geometries): return geometries == self._lh_geos.keys() @@ -480,7 +863,7 @@ def _disconnected(self, link_uri): self._clear_state() 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,22 +898,39 @@ 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, - self._rpy_to_rot(self._helper.pose_logger.rpy_rad)) - self._plot_3d.update_base_station_geos(self._lh_geos) + self._plot_3d.update_cf_pose(self._helper.pose_logger.full_pose) + self._plot_3d.update_base_station_geos(self._lh_geos, self._latest_solution) self._plot_3d.update_base_station_visibility(self._bs_data_to_estimator) + + if self._ui_mode == UiMode.geo_estimation: + self._plot_3d.update_samples(self._latest_solution) + else: + self._plot_3d.clear_samples() + self._update_position_label(self._helper.pose_logger.position) self._update_status_label(self._lh_status) self._mask_status_matrix(self._bs_available) def _update_ui(self): enabled = self._is_connected and self.is_lighthouse_deck_active - self._manage_estimate_geometry_button.setEnabled(enabled) self._change_system_type_button.setEnabled(enabled) - self._load_sys_config_button.setEnabled(enabled) - self._save_sys_config_button.setEnabled(enabled) + + self._flying_mode_button.setEnabled(enabled) + self._geo_mode_button.setEnabled(enabled) + + is_geo_visible = self._ui_mode == UiMode.geo_estimation and enabled + self._geo_estimator_widget.setVisible(is_geo_visible) + self._geo_estimator_details_widget.setVisible(is_geo_visible) def _update_position_label(self, position): if len(position) == 3: @@ -575,28 +975,6 @@ def _clear_state_indicator(self): if item is not None: item.widget().deleteLater() - def _rpy_to_rot(self, rpy): - # http://planning.cs.uiuc.edu/node102.html - # Pitch reversed compared to page above - roll = rpy[0] - pitch = rpy[1] - yaw = rpy[2] - - cg = math.cos(roll) - cb = math.cos(-pitch) - ca = math.cos(yaw) - sg = math.sin(roll) - sb = math.sin(-pitch) - sa = math.sin(yaw) - - r = [ - [ca * cb, ca * sb * sg - sa * cg, ca * sb * cg + sa * sg], - [sa * cb, sa * sb * sg + ca * cg, sa * sb * cg - ca * sg], - [-sb, cb * sg, cb * cg], - ] - - return np.array(r) - def _populate_status_matrix(self): container = self._basestation_stats_container @@ -676,19 +1054,33 @@ def _update_basestation_status_indicators(self): label.setStyleSheet(STYLE_RED_BACKGROUND) label.setToolTip('') - def _load_sys_config_button_clicked(self): + def load_sys_config_user_action(self) -> bool: + if not self._geo_estimator_widget.is_container_empty(): + dlg = QMessageBox(self) + dlg.setWindowTitle("Clear samples Confirmation") + dlg.setText("Loading a new system configuration will clear all samples. Are you sure?") + dlg.setStandardButtons(QMessageBox.StandardButton.Yes | QMessageBox.StandardButton.No) + button = dlg.exec() + + if button != QMessageBox.StandardButton.Yes: + return False + names = QFileDialog.getOpenFileName(self, 'Open file', self._helper.current_folder, FILE_REGEX_YAML) if names[0] == '': - return + return False self._helper.current_folder = os.path.dirname(names[0]) - if self._lh_config_writer is not None: - self._lh_config_writer.write_and_store_config_from_file(self._new_system_config_written_to_cf_signal.emit, - names[0]) + if self._lh_config_writer is None: + return False + + self._lh_config_writer.write_and_store_config_from_file(self._new_system_config_written_to_cf_signal.emit, + names[0]) + + return True - def _save_sys_config_button_clicked(self): + def save_sys_config_user_action(self): # Get calibration data from the Crazyflie to complete the system config data set # When the data is ready we get a callback on _calibration_read self._lh_memory_helper.read_all_calibs(self._calibration_read_signal.emit) diff --git a/src/cfclient/ui/tabs/lighthouse_tab.ui b/src/cfclient/ui/tabs/lighthouse_tab.ui index 71f5cce50..ab7d3cf88 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.ui +++ b/src/cfclient/ui/tabs/lighthouse_tab.ui @@ -6,401 +6,344 @@ 0 0 - 1753 - 763 + 1302 + 742 + + + 0 + 0 + + Plot - + - - - 0 - - - - - 6 - - - QLayout::SetDefaultConstraint - + + + - + + + + 0 + 0 + + + + + 0 + 0 + + + + Crazyflie status + + + Qt::AlignmentFlag::AlignLeading|Qt::AlignmentFlag::AlignLeft|Qt::AlignmentFlag::AlignTop + + + + + + + 150 + 0 + + + + QFrame::Shape::NoFrame + + + (0.0 , 0.0 , 0.0) + + + + + + + Position: + + + + + + + Status: + + + + + + + + 0 + 0 + + + + - + + + true + + + + + + + + - QLayout::SetDefaultConstraint + QLayout::SizeConstraint::SetMaximumSize - - - - 0 - 0 - - - - - 0 - 0 - - - - Crazyflie status - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + Geometry mode: - - - - - - - - - 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 - - - - - - - - - - - - Qt::Vertical - - - - 0 - 20 - - - - - - - - true - - - - 0 - 0 - + + + On - - - 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 - - - - - - - - - - - - - - - - - - - - - - - - 0 - 0 - - - - - 0 - 0 - + + + Off - - System Management + + true - - - QLayout::SetDefaultConstraint + + + + + + + + + + true + + + + 0 + 0 + + + + + 0 + 0 + + + + Basestation Status + + + Qt::AlignmentFlag::AlignLeading|Qt::AlignmentFlag::AlignLeft|Qt::AlignmentFlag::AlignTop + + + + + + QLayout::SizeConstraint::SetMinimumSize + + + 2 + + + + + background-color: lightpink; + + + QFrame::Shape::Box + + + + + + + + + + + 30 + 0 + + + + 1 + + + + + + + Geometry + + + + + + + Receiving + + + + + + + background-color: lightpink; + + + QFrame::Shape::Box + + + + + + + + + background-color: lightpink; + + + QFrame::Shape::Box + + + + + + + + + + Estimator + + + + + + + Calibration + + + + + + + background-color: lightpink; + + + QFrame::Shape::Box + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + System Management + + + Qt::AlignmentFlag::AlignLeading|Qt::AlignmentFlag::AlignLeft|Qt::AlignmentFlag::AlignTop + + + + QLayout::SizeConstraint::SetDefaultConstraint + + + + + + + + + Set BS channel + + + - - - - - - - Manage geometry - - - - - - - Change system type - - - - - - - Set BS channel - - - - - - - - - - - Save system config - - - - - - - Load system config - - - - - - - - - Qt::Vertical - - - - 20 - 5 - - - - - + + + Switch BS version + + - - + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + + + QLayout::SizeConstraint::SetMinimumSize + - + - Qt::Horizontal + Qt::Orientation::Horizontal @@ -412,13 +355,9 @@ - - - - - - QLayout::SetMaximumSize - + + + diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui new file mode 100644 index 000000000..bcd821a17 --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -0,0 +1,619 @@ + + + Form + + + + 0 + 0 + 420 + 793 + + + + + 0 + 0 + + + + Form + + + + + + + + + + + + + Sample collection + + + + + + + + <html><head/><body><p align="center">X-axis<br/>sample</p></body></html> + + + + + + + Refined + + + + + + + Qt::Orientation::Vertical + + + + + + + Icon + + + Qt::AlignmentFlag::AlignCenter + + + + + + + Basic + + + + + + + Icon + + + Qt::AlignmentFlag::AlignCenter + + + + + + + Icon + + + Qt::AlignmentFlag::AlignCenter + + + + + + + <html><head/><body><p align="center">Verification<br/>samples</p></body></html> + + + + + + + Icon + + + Qt::AlignmentFlag::AlignCenter + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + + <html><head/><body><p align="center">Origin<br/>sample</p></body></html> + + + Qt::AlignmentFlag::AlignLeading|Qt::AlignmentFlag::AlignLeft|Qt::AlignmentFlag::AlignVCenter + + + + + + + <html><head/><body><p align="center">XY-plane<br/>samples</p></body></html> + + + + + + + <html><head/><body><p align="center">XYZ-space<br/>samples</p></body></html> + + + + + + + Icon + + + Qt::AlignmentFlag::AlignCenter + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + Image + + + + + + + + + + + + 0 + 0 + + + + TextLabel + + + true + + + + + + + Qt::Orientation::Horizontal + + + + + + + + 0 + 0 + + + + QFrame::Shape::Panel + + + 0 + + + TextLabel + + + + + + + + + + 0 + 0 + + + + Take sample + + + + + + + + + + + + + 0 + 0 + + + + Solution status + + + + + + + 0 + 0 + + + + TextLabel + + + + + + + Qt::Orientation::Horizontal + + + + + + + + + + 0 + 0 + + + + TextLabel + + + + + + + + 0 + 0 + + + + Solution sample error: + + + + + + + + 0 + 0 + + + + TextLabel + + + + + + + + 0 + 0 + + + + Validation sample error: + + + + + + + + + Qt::Orientation::Horizontal + + + + + + + Show sample/basestation details + + + + + + + + + + + + + Session management + + + + + + Export solution + + + + + + + Export samples + + + + + + + Import solution + + + + + + + Import samples + + + + + + + + 0 + 0 + + + + Clear all samples + + + + + + + + + + + + + + Qt::Orientation::Vertical + + + + 20 + 40 + + + + + + + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_details.ui b/src/cfclient/ui/widgets/geo_estimator_details.ui new file mode 100644 index 000000000..a1dcb714b --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator_details.ui @@ -0,0 +1,117 @@ + + + Form + + + + 0 + 0 + 702 + 319 + + + + + 0 + 0 + + + + Form + + + + + + Samples + + + + + + + 0 + 0 + + + + QAbstractScrollArea::SizeAdjustPolicy::AdjustToContents + + + QAbstractItemView::SelectionMode::SingleSelection + + + QAbstractItemView::SelectionBehavior::SelectRows + + + false + + + true + + + false + + + false + + + + + + + + + + Base stations + + + + + + + 0 + 0 + + + + QAbstractScrollArea::SizeAdjustPolicy::AdjustToContents + + + QAbstractItemView::SelectionMode::SingleSelection + + + QAbstractItemView::SelectionBehavior::SelectRows + + + false + + + true + + + false + + + + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + + diff --git a/src/cfclient/ui/widgets/geo_estimator_details_widget.py b/src/cfclient/ui/widgets/geo_estimator_details_widget.py new file mode 100644 index 000000000..ff78c930a --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator_details_widget.py @@ -0,0 +1,370 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2026 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 logging +from enum import Enum + +from PyQt6 import QtCore, QtWidgets, uic, QtGui +from PyQt6.QtCore import QAbstractTableModel, QVariant, Qt, QModelIndex, QItemSelection + +from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleType +from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution +import cfclient + +__author__ = 'Bitcraze AB' +__all__ = ['GeoEstimatorDetailsWidget'] + +logger = logging.getLogger(__name__) + +(geo_estimator_details_widget_class, connect_widget_base_class) = ( + uic.loadUiType(cfclient.module_path + '/ui/widgets/geo_estimator_details.ui')) + + +class GeoEstimatorDetailsWidget(QtWidgets.QWidget, geo_estimator_details_widget_class): + """Widget for the samples and base stations details of the geometry estimator UI""" + + sample_selection_changed_signal = QtCore.pyqtSignal(int) + base_station_selection_changed_signal = QtCore.pyqtSignal(int) + do_remove_sample_signal = QtCore.pyqtSignal(int) + do_convert_to_xyz_space_sample_signal = QtCore.pyqtSignal(int) + do_convert_to_verification_sample_signal = QtCore.pyqtSignal(int) + + def __init__(self): + super(GeoEstimatorDetailsWidget, self).__init__() + self.setupUi(self) + + # Create sample details table + self._samples_details_model = SampleTableModel(self) + self._samples_table_view.setModel(self._samples_details_model) + self._samples_table_view.selectionModel().selectionChanged.connect(self._sample_selection_changed) + + header_samples = self._samples_table_view.horizontalHeader() + header_samples.setSectionResizeMode(0, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header_samples.setSectionResizeMode(1, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header_samples.setSectionResizeMode(2, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header_samples.setSectionResizeMode(3, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + + self._samples_table_view.setContextMenuPolicy(Qt.ContextMenuPolicy.CustomContextMenu) + self._samples_table_view.customContextMenuRequested.connect(self._create_sample_table_context_menu) + + # Create base station details table + self._base_stations_details_model = BaseStationTableModel(self) + self._base_stations_table_view.setModel(self._base_stations_details_model) + self._base_stations_table_view.selectionModel().selectionChanged.connect(self._base_station_selection_changed) + + header_base_stations = self._base_stations_table_view.horizontalHeader() + header_base_stations.setSectionResizeMode(0, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header_base_stations.setSectionResizeMode(1, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header_base_stations.setSectionResizeMode(2, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header_base_stations.setSectionResizeMode(3, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header_base_stations.setSectionResizeMode(4, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + + self._samples_widget.setVisible(False) + self._base_stations_widget.setVisible(False) + + def _create_sample_table_context_menu(self, point): + menu = QtWidgets.QMenu() + + delete_action = None + + item = self._samples_table_view.indexAt(point) + row = item.row() + if row >= 0: + uid = self._samples_details_model.get_uid_of_row(item.row()) + sample_type = self._samples_details_model.get_sample_type_of_row(row) + + delete_action = menu.addAction('Delete sample') + if sample_type == LhCfPoseSampleType.VERIFICATION: + change_action = menu.addAction('Change to XYZ-space sample') + else: + change_action = menu.addAction('Change to verification sample') + + action = menu.exec(self._samples_table_view.mapToGlobal(point)) + + if action == delete_action: + self.do_remove_sample_signal.emit(uid) + elif action == change_action: + if sample_type == LhCfPoseSampleType.VERIFICATION: + self.do_convert_to_xyz_space_sample_signal.emit(uid) + else: + self.do_convert_to_verification_sample_signal.emit(uid) + + def _sample_selection_changed(self, current: QItemSelection, previous: QItemSelection): + # Called from the sample details table when the selection changes + model_indexes = current.indexes() + + if len(model_indexes) > 0: + # model_indexes contains one index per column, just take the first one + row = model_indexes[0].row() + self.sample_selection_changed_signal.emit(row) + + self._base_stations_table_view.clearSelection() + else: + self.sample_selection_changed_signal.emit(-1) + + def _base_station_selection_changed(self, current: QItemSelection, previous: QItemSelection): + # Called from the base station details table when the selection changes + model_indexes = current.indexes() + + if len(model_indexes) > 0: + # model_indexes contains one index per column, just take the first one + row = model_indexes[0].row() + bs_id = self._base_stations_details_model.get_bs_id_of_row(row) + self.base_station_selection_changed_signal.emit(bs_id) + + self._samples_table_view.clearSelection() + else: + self.base_station_selection_changed_signal.emit(-1) + + def solution_ready_cb(self, solution: LighthouseGeometrySolution): + self._samples_details_model.set_solution(solution) + self._base_stations_details_model.set_solution(solution) + + # There seems to be some issues with the selection when updating the model. Reset the 3D-graph selection to + # avoid problems. + self.sample_selection_changed_signal.emit(-1) + self.base_station_selection_changed_signal.emit(-1) + + def set_selected_sample(self, index: int): + # Called from the 3D-graph when the selected sample changes + self._base_stations_table_view.clearSelection() + + if index >= 0: + self._samples_table_view.selectRow(index) + else: + self._samples_table_view.clearSelection() + + def set_selected_base_station(self, bs_id: int): + # Called from the 3D-graph when the selected base station changes + self._samples_table_view.clearSelection() + + if bs_id >= 0: + row = self._base_stations_details_model.get_row_of_bs_id(bs_id) + self._base_stations_table_view.selectRow(row) + else: + self._base_stations_table_view.clearSelection() + + def details_checkbox_state_changed(self, state: int): + enabled = state == Qt.CheckState.Checked.value + self._samples_widget.setVisible(enabled) + self._base_stations_widget.setVisible(enabled) + + +class _TableRowStatus(Enum): + INVALID = 1 + LARGE_ERROR = 2 + VERIFICATION = 3 + TOO_FEW_LINKS = 4 + + +class SampleTableModel(QAbstractTableModel): + def __init__(self, parent=None, *args): + QAbstractTableModel.__init__(self, parent) + self._headers = ['Type', 'X', 'Y', 'Z', 'Err'] + self._table_values = [] + self._uids: list[int] = [] + self._sample_types: list[LhCfPoseSampleType] = [] + self._table_highlights: list[set[_TableRowStatus]] = [] + + def rowCount(self, parent=None, *args, **kwargs): + return len(self._table_values) + + def columnCount(self, parent=None, *args, **kwargs): + return len(self._headers) + + def data(self, index: QModelIndex, role: Qt.ItemDataRole) -> QVariant: + if index.isValid(): + if role == Qt.ItemDataRole.DisplayRole: + if index.column() < len(self._table_values[index.row()]): + value = self._table_values[index.row()][index.column()] + return QVariant(value) + + if role == Qt.ItemDataRole.BackgroundRole: + color = None + if _TableRowStatus.VERIFICATION in self._table_highlights[index.row()]: + color = QtGui.QColor(255, 255, 230) + if _TableRowStatus.INVALID in self._table_highlights[index.row()]: + color = Qt.GlobalColor.gray + if _TableRowStatus.LARGE_ERROR in self._table_highlights[index.row()]: + if index.column() == 4: + color = QtGui.QColor(255, 182, 193) + + if color: + return QVariant(QtGui.QBrush(color)) + + return QVariant() + + def headerData(self, col, orientation, role=None): + if orientation == Qt.Orientation.Horizontal and role == Qt.ItemDataRole.DisplayRole: + return QVariant(self._headers[col]) + return QVariant() + + def set_solution(self, solution: LighthouseGeometrySolution): + """Set the solution and update the table values""" + self.beginResetModel() + self._table_values = [] + self._uids = [] + self._sample_types = [] + self._table_highlights = [] + + for sample in solution.samples: + status: set[_TableRowStatus] = set() + x = y = z = '--' + error = '--' + + if sample.sample_type == LhCfPoseSampleType.VERIFICATION: + status.add(_TableRowStatus.VERIFICATION) + + if sample.is_valid: + if sample.has_pose: + error = f'{sample.error_distance * 1000:.1f} mm' + pose = sample.pose + x = f'{pose.translation[0]:.2f}' + y = f'{pose.translation[1]:.2f}' + z = f'{pose.translation[2]:.2f}' + + if sample.is_error_large: + status.add(_TableRowStatus.LARGE_ERROR) + else: + error = f'{sample.status}' + status.add(_TableRowStatus.INVALID) + + self._table_values.append([ + f'{sample.sample_type}', + x, + y, + z, + error, + ]) + self._uids.append(sample.uid) + self._sample_types.append(sample.sample_type) + self._table_highlights.append(status) + + self.endResetModel() + + def get_uid_of_row(self, row: int) -> int: + """Get the sample UID for a given row""" + if 0 <= row < len(self._uids): + return self._uids[row] + + raise IndexError("Row index out of range") + + def get_sample_type_of_row(self, row: int) -> LhCfPoseSampleType: + """Get the sample type for a given row""" + if 0 <= row < len(self._sample_types): + return self._sample_types[row] + + raise IndexError("Row index out of range") + + +class BaseStationTableModel(QAbstractTableModel): + def __init__(self, parent=None, *args): + QAbstractTableModel.__init__(self, parent) + self._headers = ['Id', 'X', 'Y', 'Z', 'Samples', 'Links'] + self._table_values = [] + self._table_highlights: list[set[_TableRowStatus]] = [] + + def rowCount(self, parent=None, *args, **kwargs): + return len(self._table_values) + + def columnCount(self, parent=None, *args, **kwargs): + return len(self._headers) + + def data(self, index: QModelIndex, role: Qt.ItemDataRole) -> QVariant: + if index.isValid(): + if role == Qt.ItemDataRole.DisplayRole: + if index.column() < len(self._table_values[index.row()]): + value = self._table_values[index.row()][index.column()] + return QVariant(value) + + if role == Qt.ItemDataRole.BackgroundRole: + color = None + if _TableRowStatus.TOO_FEW_LINKS in self._table_highlights[index.row()]: + if index.column() == 5: + color = QtGui.QColor(255, 182, 193) + + if color: + return QVariant(QtGui.QBrush(color)) + + return QVariant() + + def headerData(self, col, orientation, role=None): + if orientation == Qt.Orientation.Horizontal and role == Qt.ItemDataRole.DisplayRole: + return QVariant(self._headers[col]) + return QVariant() + + def set_solution(self, solution: LighthouseGeometrySolution): + """Set the solution and update the table values""" + self.beginResetModel() + self._table_values = [] + self._table_highlights = [] + + # Dictionary keys may not be ordered, sort by base station ID + for bs_id, pose in sorted(solution.bs_poses.items()): + status: set[_TableRowStatus] = set() + + x = f'{pose.translation[0]:.2f}' + y = f'{pose.translation[1]:.2f}' + z = f'{pose.translation[2]:.2f}' + + link_count = len(solution.link_count[bs_id]) + if link_count < solution.link_count_ok_threshold: + status.add(_TableRowStatus.TOO_FEW_LINKS) + + samples_containing_bs = solution.bs_sample_count[bs_id] + + self._table_values.append([ + bs_id + 1, + x, + y, + z, + samples_containing_bs, + link_count, + ]) + self._table_highlights.append(status) + + self.endResetModel() + + def get_bs_id_of_row(self, row: int) -> int: + """Get the base station ID for a given row""" + if 0 <= row < len(self._table_values): + bs_id = self._table_values[row][0] - 1 # IDs are 1-based in the table + return bs_id + + return -1 + + def get_row_of_bs_id(self, bs_id: int) -> int: + """Get the row index for a given base station ID""" + for row, values in enumerate(self._table_values): + if values[0] - 1 == bs_id: # IDs are 1-based in the table + return row + + return -1 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 000000000..2a7eb79e8 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 000000000..3ce0c4a02 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 000000000..640765c6a 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 000000000..b4936b2a7 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/checkmark.png b/src/cfclient/ui/widgets/geo_estimator_resources/checkmark.png new file mode 100644 index 000000000..d0573f77f Binary files /dev/null and b/src/cfclient/ui/widgets/geo_estimator_resources/checkmark.png differ diff --git a/src/cfclient/ui/widgets/geo_estimator_resources/crossmark.png b/src/cfclient/ui/widgets/geo_estimator_resources/crossmark.png new file mode 100644 index 000000000..5cd1c386a Binary files /dev/null and b/src/cfclient/ui/widgets/geo_estimator_resources/crossmark.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 000000000..42feda16d --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -0,0 +1,643 @@ +#!/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 cfclient.ui.widgets.info_label import InfoLabel +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', + 'Put the Crazyflie where you want the origin of your coordinate system.', + 'Start measurement') + X_AXIS = ('bslh_2.png', + f'Put the Crazyflie on the positive X-axis, exactly {REFERENCE_DIST} meters from the ' + + 'origin. This sample will be used to define the X-axis as well as scaling of the system.', + 'Start measurement') + XY_PLANE = ('bslh_3.png', + 'Put the Crazyflie somewhere in the XY-plane, but not on the X-axis. This position is used to map ' + + 'the XY-plane to the floor. You can sample multiple positions to get a more precise definition.', + 'Start measurement') + VERIFICATION = ('bslh_4.png', + 'Sample points to be used for verification of the geometry. Sample by rotating the Crazyflie ' + + 'quickly left-right around the Z-axis and then holding it still for a second, or ' + + 'optionally by clicking the button.', + 'Sample position') + XYZ_SPACE = ('bslh_4.png', + 'Sample points in the space to be used for refining the geometry. You need at least two base ' + + 'stations visible in each sample. Sample by rotating the Crazyflie quickly left-right around the ' + + 'Z-axis and then holding it still for a second, or optionally by clicking the button.', + 'Sample position') + + def __init__(self, image, instructions, button_text): + self.image = image + self.instructions = instructions + self.button_text = button_text + + 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, + self.VERIFICATION] + 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" + + +class _UploadStatus(Enum): + NOT_STARTED = "not_started" + UPLOADING_SAMPLE_SOLUTION = "uploading_sample_solution" + UPLOADING_FILE_CONFIG = "uploading_file_config" + SAMPLE_SOLUTION_DONE = "sample_solution_done" + FILE_CONFIG_DONE = "file_config_done" + + +STYLE_GREEN_BACKGROUND = "background-color: lightgreen;" +STYLE_RED_BACKGROUND = "background-color: lightpink;" +STYLE_YELLOW_BACKGROUND = "background-color: lightyellow;" +STYLE_NO_BACKGROUND = "background-color: none;" + + +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) + _start_solving_signal = QtCore.pyqtSignal() + 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_measure.clicked.connect(self._measure) + + self._clear_all_button.clicked.connect(self._clear_all) + self._import_samples_button.clicked.connect(lambda: self._load_samples_from_file(use_session_path=True)) + self._export_samples_button.clicked.connect(self._save_samples_to_file) + + self._upload_status = _UploadStatus.NOT_STARTED + + self._import_solution_button.clicked.connect(self._start_geo_file_upload) + self._export_solution_button.clicked.connect(self._lighthouse_tab.save_sys_config_user_action) + + 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._origin_radio.setChecked(True) + + self._start_solving_signal.connect(self._start_solving_cb) + self.solution_ready_signal.connect(self._solution_ready_cb) + self._is_solving = False + self._solver_thread = None + + self._update_step_ui() + self._update_ui_reading(False) + self._update_solution_info() + + self._origin_radio.clicked.connect(lambda: self._change_step(_CollectionStep.ORIGIN)) + self._x_axis_radio.clicked.connect(lambda: self._change_step(_CollectionStep.X_AXIS)) + self._xy_plane_radio.clicked.connect(lambda: self._change_step(_CollectionStep.XY_PLANE)) + self._xyz_space_radio.clicked.connect(lambda: self._change_step(_CollectionStep.XYZ_SPACE)) + self._verification_radio.clicked.connect(lambda: self._change_step(_CollectionStep.VERIFICATION)) + self._details_checkbox.setChecked(False) + + self._basic_steps_info_label = InfoLabel("This is information about basic steps. TODO update", + self._basic_steps_section_label) + self._refined_steps_info_label = InfoLabel("This is information about refined steps. TODO update", + self._refined_steps_section_label) + self._solution_status_info_label = InfoLabel("This is information about the solution status. TODO update", + self._solution_status_group_box) + self._session_management_info_label = InfoLabel("This is information about session management. TODO update", + self._session_management_group_box) + + def _start_geo_file_upload(self): + if self._lighthouse_tab.load_sys_config_user_action(): + self._upload_status = _UploadStatus.UPLOADING_FILE_CONFIG + + # Clear all samples as the new configuration is based on some other (unknown) set of samples + self.new_session() + + 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, + is_starting_estimation_cb=( + self._start_solving_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): + if not self.is_container_empty(): + 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 is_container_empty(self) -> bool: + """Check if the container has any samples""" + return self._container.is_empty() + + def remove_sample(self, uid: int) -> None: + """Delete a sample by its unique ID""" + self._container.remove_sample(uid) + + def convert_to_xyz_space_sample(self, uid: int) -> None: + """Convert a sample to an xyz-space sample by its unique ID""" + self._container.convert_to_xyz_space_sample(uid) + + def convert_to_verification_sample(self, uid: int) -> None: + """Convert a sample to a verification sample by its unique ID""" + self._container.convert_to_verification_sample(uid) + + def cf_connected_cb(self, link_uri: str): + """Callback when the Crazyflie has connected""" + self._upload_status = _UploadStatus.NOT_STARTED + self._update_solution_info() + + def geometry_has_been_read_back_cb(self): + """Callback when the geometry has been read back from the Crazyflie after an upload, that is the full + round-trip is done. Called from the tab.""" + if self._upload_status == _UploadStatus.UPLOADING_SAMPLE_SOLUTION: + self._upload_status = _UploadStatus.SAMPLE_SOLUTION_DONE + elif self._upload_status == _UploadStatus.UPLOADING_FILE_CONFIG: + self._upload_status = _UploadStatus.FILE_CONFIG_DONE + + self._update_solution_info() + + def _load_samples_from_file(self, use_session_path=False): + path = self._session_path if use_session_path else self._helper.current_folder + names = QFileDialog.getOpenFileName(self, 'Load session', path, self.FILE_REGEX_YAML) + + if names[0] == '': + return + + if not use_session_path: + # If not using the session path, update the current folder + self._helper.current_folder = os.path.dirname(names[0]) + + file_name = names[0] + with open(file_name, 'r', encoding='UTF8') as handle: + self._container.populate_from_file_yaml(handle) + + def _save_samples_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 in [_CollectionStep.XYZ_SPACE, _CollectionStep.VERIFICATION]: + 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._set_label_icon(self._step_image, step.image) + self._step_instructions.setText(step.instructions) + self._step_info.setText('') + self._step_measure.setText(step.button_text) + + self._update_solution_info() + + def _set_label_icon(self, label: QtWidgets.QLabel, icon_file_name: str): + """Set the icon of a label widget""" + path = cfclient.module_path + '/ui/widgets/geo_estimator_resources/' + icon_file_name + label.setPixmap(QtGui.QPixmap(path)) + + 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._origin_radio.setEnabled(is_enabled) + self._x_axis_radio.setEnabled(is_enabled) + self._xy_plane_radio.setEnabled(is_enabled) + self._xyz_space_radio.setEnabled(is_enabled) + self._verification_radio.setEnabled(is_enabled) + + self._import_samples_button.setEnabled(is_enabled) + self._export_samples_button.setEnabled(is_enabled) + self._import_solution_button.setEnabled(is_enabled) + self._export_solution_button.setEnabled(is_enabled) + self._clear_all_button.setEnabled(is_enabled) + + def _get_step_icon(self, is_valid: bool) -> str: + """Get the icon file name based on validity""" + if is_valid: + return 'checkmark.png' + else: + return 'crossmark.png' + + def _update_solution_info(self): + solution = self._latest_solution + + self._set_label_icon(self._origin_icon, self._get_step_icon(solution.is_origin_sample_valid)) + self._set_label_icon(self._x_axis_icon, self._get_step_icon(solution.is_x_axis_samples_valid)) + self._set_label_icon(self._xy_plane_icon, self._get_step_icon(solution.is_xy_plane_samples_valid)) + self._xyz_space_icon.setText(f'({self._container.xyz_space_sample_count()})') + self._verification_icon.setText(f'({self._container.verification_sample_count()})') + + if self._is_solving: + self._solution_status_info.setText('Updating...') + self._set_background_none(self._solution_status_info) + else: + background_color_is_ok = solution.progress_is_ok + + solution_error = '--' + verification_error = '--' + + if solution.progress_is_ok: + if self._upload_status == _UploadStatus.UPLOADING_SAMPLE_SOLUTION: + self._solution_status_info.setText('Uploading...') + elif self._upload_status == _UploadStatus.SAMPLE_SOLUTION_DONE: + self._solution_status_info.setText('Uploaded') + + solution_error = f'{solution.error_stats.max * 1000:.1f} mm (max)' + + if solution.verification_stats: + verification_error = f'{solution.verification_stats.max * 1000:.1f} mm (max)' + else: + no_samples = not solution.contains_samples + if no_samples and self._upload_status == _UploadStatus.FILE_CONFIG_DONE: + self._solution_status_info.setText('Uploaded (imported config)') + background_color_is_ok = True + else: + self._solution_status_info.setText('Not enough samples') + + self._solution_status_max_error.setText(f'{solution_error}') + self._solution_status_verification_error.setText(f'{verification_error}') + self._set_background_color(self._solution_status_info, background_color_is_ok) + + def _notify_user(self, notification_type: _UserNotificationType): + timeout = 1000 + 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) + timeout = 3000 + + self._user_notification_clear_timer.stop() + self._user_notification_clear_timer.start(timeout) + + def _user_notification_clear(self): + self._sample_collection_box.setStyleSheet('') + + def _set_background_none(self, widget: QtWidgets.QWidget): + widget.setStyleSheet(STYLE_NO_BACKGROUND) + + 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_single_sample() + case _CollectionStep.VERIFICATION: + self._measure_single_sample() + + 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_single_sample(self): + """Measure a single sample. Used both for xyz-space and verification""" + logger.debug("Measuring single sample...") + 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) + self._user_notification_signal.emit(_UserNotificationType.PENDING) + + 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 _start_solving_cb(self): + self._is_solving = True + self._update_solution_info() + + def _solution_ready_cb(self, solution: LighthouseGeometrySolution): + # Called when the solver thread has a new solution ready + self._is_solving = False + self._latest_solution = solution + + 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: + no_samples = not solution.contains_samples + if (self._upload_status == _UploadStatus.UPLOADING_FILE_CONFIG or + self._upload_status == _UploadStatus.FILE_CONFIG_DONE) and no_samples: + # If we imported a config file and started an upload all samples are cleared and we will end up here + # when the solver is done. We don't want to change the upload status in this case as the status will + # show that there are no samples. + pass + else: + self._upload_status = _UploadStatus.UPLOADING_SAMPLE_SOLUTION + + self._upload_geometry(solution.bs_poses) + + self._update_solution_info() + + 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_single_sample() + + def _single_sample_ready_cb(self, sample: LhCfPoseSample): + self._user_notification_signal.emit(_UserNotificationType.SUCCESS) + self._container_updated_signal.emit() + match self._current_step: + case _CollectionStep.XYZ_SPACE: + self._container.append_xyz_space_samples([sample]) + case _CollectionStep.VERIFICATION: + self._container.append_verification_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/widgets/info_label.py b/src/cfclient/ui/widgets/info_label.py new file mode 100644 index 000000000..a197e3b52 --- /dev/null +++ b/src/cfclient/ui/widgets/info_label.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2026 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. + +from enum import Enum +from PyQt6.QtWidgets import QLabel, QWidget, QToolTip +from PyQt6.QtCore import QObject, QEvent + + +class InfoLabel(QLabel): + """A label with an information icon and a tooltip.""" + + class Position(Enum): + TOP_LEFT = 1 + TOP_RIGHT = 2 + BOTTOM_LEFT = 3 + BOTTOM_RIGHT = 4 + + ICON_WIDTH = 16 + ICON_HEIGHT = 16 + MARGIN = 0 + + def __init__(self, tooltip: str, parent: QWidget, position: Position = Position.TOP_RIGHT, + v_margin: int = MARGIN, h_margin: int = MARGIN): + super().__init__(parent) + + self._v_margin = v_margin + self._h_margin = h_margin + + self._event_filter = _EventFilter(self, position) + parent.installEventFilter(self._event_filter) + + self.setToolTip(tooltip) + + info_pixmap = self.style().StandardPixmap.SP_MessageBoxInformation + info_icon = self.style().standardIcon(info_pixmap).pixmap(self.ICON_WIDTH, self.ICON_HEIGHT) + self.setPixmap(info_icon) + + def mousePressEvent(self, event): + """Override mouse press event to show tooltip on click.""" + QToolTip.showText(event.globalPosition().toPoint(), self.toolTip(), self) + + +class _EventFilter(QObject): + def __init__(self, info_label: 'InfoLabel', position: InfoLabel.Position): + super().__init__() + self._info_label = info_label + self._position = position + + def eventFilter(self, obj, event): + if event.type() == QEvent.Type.Resize: + self._update_position() + return super().eventFilter(obj, event) + + def _update_position(self): + parent = self._info_label.parent() + if parent is None: + return + x, y = 0, 0 + if self._position == InfoLabel.Position.TOP_LEFT: + x, y = self._info_label._h_margin, self._info_label._v_margin + elif self._position == InfoLabel.Position.TOP_RIGHT: + x = parent.width() - self._info_label.ICON_WIDTH - self._info_label._h_margin + y = self._info_label._v_margin + elif self._position == InfoLabel.Position.BOTTOM_LEFT: + x = self._info_label._h_margin + y = parent.height() - self._info_label.ICON_HEIGHT - self._info_label._v_margin + elif self._position == InfoLabel.Position.BOTTOM_RIGHT: + x = parent.width() - self._info_label.ICON_WIDTH - self._info_label._h_margin + y = parent.height() - self._info_label.ICON_HEIGHT - self._info_label._v_margin + self._info_label.move(x, y) diff --git a/src/cfclient/ui/wizards/__init__.py b/src/cfclient/ui/wizards/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/cfclient/ui/wizards/bslh_1.png b/src/cfclient/ui/wizards/bslh_1.png deleted file mode 100644 index 0b1d1f754..000000000 Binary files a/src/cfclient/ui/wizards/bslh_1.png and /dev/null differ diff --git a/src/cfclient/ui/wizards/bslh_2.png b/src/cfclient/ui/wizards/bslh_2.png deleted file mode 100644 index 56dfb5fb6..000000000 Binary files a/src/cfclient/ui/wizards/bslh_2.png and /dev/null differ diff --git a/src/cfclient/ui/wizards/bslh_3.png b/src/cfclient/ui/wizards/bslh_3.png deleted file mode 100644 index e5f385e4e..000000000 Binary files a/src/cfclient/ui/wizards/bslh_3.png and /dev/null differ diff --git a/src/cfclient/ui/wizards/bslh_4.png b/src/cfclient/ui/wizards/bslh_4.png deleted file mode 100644 index 434ff48e1..000000000 Binary files a/src/cfclient/ui/wizards/bslh_4.png and /dev/null differ diff --git a/src/cfclient/ui/wizards/bslh_5.png b/src/cfclient/ui/wizards/bslh_5.png deleted file mode 100644 index c20847a46..000000000 Binary files a/src/cfclient/ui/wizards/bslh_5.png and /dev/null differ diff --git a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py b/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py deleted file mode 100644 index 63ef456d1..000000000 --- a/src/cfclient/ui/wizards/lighthouse_geo_bs_estimation_wizard.py +++ /dev/null @@ -1,465 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2022-2023 Bitcraze AB -# -# 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. - -""" -Wizard to estimate the geometry of the lighthouse base stations. -Used in the lighthouse tab from the manage geometry dialog -""" - -from __future__ import annotations - -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 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, LhCfPoseSample - -from PyQt6 import QtCore, QtWidgets, QtGui -import time - - -REFERENCE_DIST = 1.0 -ITERATION_MAX_NR = 2 -DEFAULT_RECORD_TIME = 20 -TIMEOUT_TIME = 2000 -STRING_PAD_TOTAL = 6 -WINDOW_STARTING_WIDTH = 780 -WINDOW_STARTING_HEIGHT = 720 -SPACER_LABEL_HEIGHT = 27 -PICTURE_WIDTH = 640 - - -class LighthouseBasestationGeometryWizard(QtWidgets.QWizard): - def __init__(self, cf, ready_cb, parent=None, *args): - super(LighthouseBasestationGeometryWizard, self).__init__(parent) - self.cf = cf - 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()) - - def reset(self): - self.setWindowFlags(self.windowFlags() & ~QtCore.Qt.WindowType.WindowContextHelpButtonHint) - self.setWindowFlags(self.windowFlags() & ~QtCore.Qt.WindowType.WindowCloseButtonHint) - - if not self.wizard_opened_first_time: - self.removePage(0) - 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 - 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.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.setWindowTitle("Lighthouse Base Station Geometry Wizard") - self.resize(WINDOW_STARTING_WIDTH, WINDOW_STARTING_HEIGHT) - - -class LighthouseBasestationGeometryWizardBasePage(QtWidgets.QWizardPage): - - def __init__(self, cf: Crazyflie, show_add_measurements=False, parent=None): - super(LighthouseBasestationGeometryWizardBasePage, self).__init__(parent) - self.show_add_measurements = show_add_measurements - self.cf = cf - self.layout = QtWidgets.QVBoxLayout() - - self.explanation_picture = QtWidgets.QLabel() - self.explanation_picture.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) - self.layout.addWidget(self.explanation_picture) - - self.explanation_text = QtWidgets.QLabel() - self.explanation_text.setText(' ') - self.explanation_text.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) - self.layout.addWidget(self.explanation_text) - - self.layout.addStretch() - - self.extra_layout_field() - - self.status_text = QtWidgets.QLabel() - self.status_text.setFont(QtGui.QFont('Courier New', 10)) - self.status_text.setText(self.str_pad('')) - self.status_text.setFrameStyle(QtWidgets.QFrame.Shape.Panel | QtWidgets.QFrame.Shadow.Plain) - self.layout.addWidget(self.status_text) - - self.start_action_button = QtWidgets.QPushButton("Start Measurement") - self.start_action_button.clicked.connect(self._action_btn_clicked) - action_button_h_box = QtWidgets.QHBoxLayout() - action_button_h_box.addStretch() - action_button_h_box.addWidget(self.start_action_button) - action_button_h_box.addStretch() - self.layout.addLayout(action_button_h_box) - self.setLayout(self.layout) - self.is_done = False - self.too_few_bs = False - self.timeout_timer = QtCore.QTimer() - self.timeout_timer.timeout.connect(self._timeout_cb) - self.reader = LighthouseSweepAngleAverageReader(self.cf, self._ready_cb) - self.recorded_angle_result = None - self.recorded_angles_result: list[LhCfPoseSample] = [] - - def isComplete(self): - return self.is_done and (self.too_few_bs is not True) - - def extra_layout_field(self): - self.spacer = QtWidgets.QLabel() - self.spacer.setText(' ') - self.spacer.setFixedSize(50, SPACER_LABEL_HEIGHT) - self.layout.addWidget(self.spacer) - - def _action_btn_clicked(self): - self.is_done = False - self.reader.start_angle_collection() - self.timeout_timer.start(TIMEOUT_TIME) - self.status_text.setText(self.str_pad('Collecting sweep angles...')) - self.start_action_button.setDisabled(True) - - def _timeout_cb(self): - if self.is_done is not True: - self.status_text.setText(self.str_pad('No sweep angles recorded! \n' + - 'Make sure that the lighthouse base stations are turned on!')) - self.reader.stop_angle_collection() - self.start_action_button.setText("Restart Measurement") - self.start_action_button.setDisabled(False) - elif self.too_few_bs: - 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()) - - if amount_of_basestations < 2: - self.status_text.setText(self.str_pad('Recording Done!' + - f' Visible Base stations: {self.visible_basestations}\n' + - 'Received too few base stations,' + - 'we need at least two. Please try again!')) - self.too_few_bs = True - self.is_done = True - if self.show_add_measurements and len(self.recorded_angles_result) > 0: - self.too_few_bs = False - self.completeChanged.emit() - self.start_action_button.setText("Restart Measurement") - self.start_action_button.setDisabled(False) - else: - self.too_few_bs = False - status_text_string = f'Recording Done! Visible Base stations: {self.visible_basestations}\n' - if self.show_add_measurements: - self.recorded_angles_result.append(self.get_sample()) - status_text_string += f'Total measurements added: {len(self.recorded_angles_result)}\n' - self.status_text.setText(self.str_pad(status_text_string)) - self.is_done = True - self.completeChanged.emit() - - if self.show_add_measurements: - self.start_action_button.setText("Add more measurements") - self.start_action_button.setDisabled(False) - else: - self.start_action_button.setText("Restart Measurement") - self.start_action_button.setDisabled(False) - - def get_sample(self): - return self.recorded_angle_result - - def str_pad(self, string_msg): - new_string_msg = string_msg - - if string_msg.count('\n') < STRING_PAD_TOTAL: - for i in range(STRING_PAD_TOTAL-string_msg.count('\n')): - new_string_msg += '\n' - - return new_string_msg - - -class RecordOriginSamplePage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, parent=None): - super(RecordOriginSamplePage, self).__init__(cf) - 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) - - -class RecordXAxisSamplePage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, parent=None): - super(RecordXAxisSamplePage, self).__init__(cf) - 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.') - pixmap = QtGui.QPixmap(cfclient.module_path + "/ui/wizards/bslh_2.png") - pixmap = pixmap.scaledToWidth(PICTURE_WIDTH) - self.explanation_picture.setPixmap(pixmap) - - -class RecordXYPlaneSamplesPage(LighthouseBasestationGeometryWizardBasePage): - def __init__(self, cf: Crazyflie, parent=None): - super(RecordXYPlaneSamplesPage, self).__init__(cf, 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.') - pixmap = QtGui.QPixmap(cfclient.module_path + "/ui/wizards/bslh_3.png") - pixmap = pixmap.scaledToWidth(PICTURE_WIDTH) - self.explanation_picture.setPixmap(pixmap) - - 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') - 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() - - 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.start_action_button.setDisabled(True) - - 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 - - -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())