diff --git a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py index f8658bdc7..5206d109d 100644 --- a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py +++ b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py @@ -34,7 +34,6 @@ 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'] @@ -129,7 +128,6 @@ def set_current_geos(self, geos): 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) @@ -137,7 +135,6 @@ def __init__(self, lighthouse_tab, *args): 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: @@ -149,15 +146,11 @@ def __init__(self, lighthouse_tab, *args): 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 = {} @@ -180,11 +173,6 @@ 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 = {} @@ -200,11 +188,6 @@ def _sweep_angles_received_and_averaged_cb(self, averaged_angles): 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() diff --git a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.ui b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.ui index 86fb1e6d5..ffc973910 100644 --- a/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.ui +++ b/src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.ui @@ -21,13 +21,6 @@ - - - - Estimate Geometry - - - diff --git a/src/cfclient/ui/tabs/lighthouse_tab.py b/src/cfclient/ui/tabs/lighthouse_tab.py index 8d2969942..2672219bd 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.py +++ b/src/cfclient/ui/tabs/lighthouse_tab.py @@ -31,6 +31,7 @@ """ import logging +from enum import Enum from PyQt6 import uic from PyQt6.QtCore import Qt, pyqtSignal, QTimer @@ -41,10 +42,15 @@ import cfclient from cfclient.ui.tab_toolbox import TabToolbox +from cfclient.ui.widgets.geo_estimator_widget import GeoEstimatorWidget from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import LighthouseMemHelper from cflib.localization import LighthouseConfigWriter from cflib.localization import LighthouseConfigFileManager +from cflib.localization import LighthouseGeometrySolution +from cflib.localization import LhCfPoseSampleType + +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 @@ -80,30 +86,41 @@ 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=False, interactive=False, symbol: str = 'disc'): self._scene = the_scene self._color = color self._text = text + self._position = [0.0, 0, 0] + self._symbol = symbol self._marker = scene.visuals.Markers( pos=np.array([[0, 0, 0]]), parent=self._scene, - face_color=self._color) + face_color=self._color, + symbol=self._symbol) - self._x_axis = scene.visuals.Line( - pos=np.array([[0, 0, 0], [0, 0, 0]]), - color=self.COL_X_AXIS, - parent=self._scene) + if interactive: + self._marker.interactive = True - self._y_axis = scene.visuals.Line(pos=np.array( - [[0, 0, 0], [0, 0, 0]]), - color=self.COL_Y_AXIS, - parent=self._scene) + if axis_visible: + self._x_axis = scene.visuals.Line( + pos=np.array([[0, 0, 0], [0, 0, 0]]), + color=self.COL_X_AXIS, + parent=self._scene) - self._z_axis = scene.visuals.Line( - pos=np.array([[0, 0, 0], [0, 0, 0]]), - color=self.COL_Z_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) + + self._z_axis = scene.visuals.Line( + pos=np.array([[0, 0, 0], [0, 0, 0]]), + color=self.COL_Z_AXIS, + parent=self._scene) + else: + self._x_axis = None + self._y_axis = None + self._z_axis = None self._label = None if self._text: @@ -114,38 +131,122 @@ def __init__(self, the_scene, color, text=None): parent=self._scene) 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): + return + self._position = position + + self._marker.set_data(pos=np.array([position]), face_color=self._color, symbol=self._symbol) if self._label: self._label.pos = self.LABEL_OFFSET + position - 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) + if self._x_axis: + 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) + + 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) - 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) + 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) - 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) + 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): + return visual == self._marker -class Plot3dLighthouse(scene.SceneCanvas): + +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): BS_BRUSH_VISIBLE = np.array((0.2, 0.5, 0.2)) BS_BRUSH_NOT_VISIBLE = np.array((0.8, 0.5, 0.5)) + def __init__(self, the_scene, text=None): + super().__init__(the_scene, self.BS_BRUSH_NOT_VISIBLE, text, axis_visible=True) + + def set_receiving_status(self, visible: bool): + if visible: + self.set_color(self.BS_BRUSH_VISIBLE) + else: + self.set_color(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: + # always update lines when highlighted as base station positions may have changed + self.set_color(self.HIGHLIGHT_BRUSH) + 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._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,7 +257,7 @@ class Plot3dLighthouse(scene.SceneCanvas): TEXT_OFFSET = np.array((0.0, 0, 0.25)) - def __init__(self): + def __init__(self, sample_clicked_signal: pyqtSignal(int)): scene.SceneCanvas.__init__(self, keys=None) self.unfreeze() @@ -169,7 +270,11 @@ def __init__(self): self._cf = None self._base_stations = {} + self._samples = [] + self.selected_sample_index = -1 + self.events.mouse_press.connect(self.on_mouse_press) + self._sample_clicked_signal = sample_clicked_signal self.freeze() plane_size = 10 @@ -184,6 +289,14 @@ def __init__(self): self._addArrows(1, 0.02, 0.1, 0.1, self._view.scene) + def on_mouse_press(self, event): + visual = self.visual_at(event.pos) + for index, sample in enumerate(self._samples): + if sample.is_same_visual(visual): + clicked_index = index + self._sample_clicked_signal.emit(clicked_index) + break + 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. @@ -226,13 +339,13 @@ def _addArrows(self, length, width, head_length, head_width, parent): def update_cf_pose(self, position, rot): if not self._cf: - self._cf = MarkerPose(self._view.scene, self.POSITION_BRUSH) + self._cf = CfMarkerPose(self._view.scene) self._cf.set_pose(position, rot) def update_base_station_geos(self, geos): for id, geo in geos.items(): 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) geos_to_remove = self._base_stations.keys() - geos.keys() @@ -242,10 +355,7 @@ def update_base_station_geos(self, geos): 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 +365,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)) + + 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 _mix(self, col1, col2, mix): - return col1 * mix + col2 * (1.0 - mix) + 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 +418,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 +434,17 @@ 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) def __init__(self, helper): super(LighthouseTab, self).__init__(helper, 'Lighthouse Positioning') self.setupUi(self) + self._geo_estimator_widget = GeoEstimatorWidget(self) + self._geometry_area.addWidget(self._geo_estimator_widget) + self._geo_estimator_widget.solution_ready_signal.connect(self._solution_updated_cb) + self._geo_estimator_widget.sample_selection_changed_signal.connect(self._sample_selection_changed_cb) + # 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 +454,7 @@ 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_widget.set_selected_sample) # Connect the Crazyflie API callbacks to the signals self._helper.cf.connected.add_callback(self._connected_signal.emit) @@ -355,19 +506,32 @@ def __init__(self, helper): self._load_sys_config_button.clicked.connect(self._load_sys_config_button_clicked) self._save_sys_config_button.clicked.connect(self._save_sys_config_button_clicked) + self._ui_mode = UiMode.flying + self._geo_mode_button.toggled.connect(lambda enabled: self._change_ui_mode(enabled)) + self._is_connected = False self._update_ui() - def write_and_store_geometry(self, geometries): + self._pending_geo_update = None + + 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() + 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_geometry_dialog(self): self._basestation_geometry_dialog.reset() @@ -378,7 +542,7 @@ def _show_basestation_mode_dialog(self): self._basestation_mode_dialog.show() def _set_up_plots(self): - self._plot_3d = Plot3dLighthouse() + self._plot_3d = Plot3dLighthouse(self._sample_clicked_signal) self._plot_layout.addWidget(self._plot_3d.native) def _connected(self, link_uri): @@ -386,6 +550,7 @@ def _connected(self, link_uri): logger.debug("Crazyflie connected to {}".format(link_uri)) self._basestation_geometry_dialog.reset() + self._flying_mode_button.setChecked(True) self._is_connected = True if self._helper.cf.param.get_value('deck.bcLighthouse4') == '1': @@ -429,6 +594,13 @@ def _geometry_read_cb(self, geometries): 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 selected sample in the geo estimator widget changes""" + self._plot_3d.selected_sample_index = sample_index + def _is_matching_current_geo_data(self, geometries): return geometries == self._lh_geos.keys() @@ -481,6 +653,7 @@ def _disconnected(self, link_uri): self._update_graphics() self._plot_3d.clear() self._basestation_geometry_dialog.close() + self._flying_mode_button.setChecked(True) self.is_lighthouse_deck_active = False self._is_connected = False self._update_ui() @@ -515,12 +688,26 @@ 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_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) @@ -532,6 +719,10 @@ def _update_ui(self): self._load_sys_config_button.setEnabled(enabled) self._save_sys_config_button.setEnabled(enabled) + self._mode_group.setEnabled(enabled) + + self._geo_estimator_widget.setVisible(self._ui_mode == UiMode.geo_estimation and enabled) + def _update_position_label(self, position): if len(position) == 3: coordinate = "({:0.2f}, {:0.2f}, {:0.2f})".format( diff --git a/src/cfclient/ui/tabs/lighthouse_tab.ui b/src/cfclient/ui/tabs/lighthouse_tab.ui index 71f5cce50..fa9a06877 100644 --- a/src/cfclient/ui/tabs/lighthouse_tab.ui +++ b/src/cfclient/ui/tabs/lighthouse_tab.ui @@ -6,399 +6,411 @@ 0 0 - 1753 - 763 + 1302 + 742 Plot - + - - - 0 + + + QLayout::SetDefaultConstraint - - - - 6 + + + + + 0 + 0 + - - QLayout::SetDefaultConstraint + + + 0 + 0 + - - - - QLayout::SetDefaultConstraint - - - - - - 0 - 0 - - - - - 0 - 0 - - - - Crazyflie status - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - + + Crazyflie status + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + + + - - - - - - - Status: - - - - - - - - 0 - 0 - - - - - 200 - 0 - - - - - - - - true - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - Position: - - - - - - - - 150 - 0 - - - - QFrame::NoFrame - - - (0.0 , 0.0 , 0.0) - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - + + + Status: + + + + + + + + 0 + 0 + + + + + 200 + 0 + + + + - + + + true + + - + - Qt::Vertical + Qt::Horizontal - 0 + 40 20 - - - - - - true - - - - 0 - 0 - - - - - 0 - 0 - - - - Basestation Status - - + + + + + + + Position: + + + + + + + + 150 + 0 + + + + QFrame::NoFrame + + + (0.0 , 0.0 , 0.0) + + + - - - - - - - QLayout::SetMinimumSize - - - 2 - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - - 30 - 0 - - - - 1 - - - - - - - - 100 - 0 - - - - Geometry - - - - - - - Receiving - - - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - - 100 - 0 - - - - Estimator - - - - - - - Calibration - - - - - - - background-color: lightpink; - - - QFrame::Box - - - - - - - - - - - - - - - - - - + + + Qt::Horizontal + + + + 40 + 20 + + + - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - System Management - - - - QLayout::SetDefaultConstraint + + + + + + + + + + true + + + + 0 + 0 + + + + + 0 + 0 + + + + Basestation Status + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + + + QLayout::SetMinimumSize + + + 2 + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + + 30 + 0 + + + + 1 + + + + + + + + 100 + 0 + + + + Geometry + + + + + + + Receiving + + + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + 100 + 0 + + + + Estimator + + + + + + + Calibration + + + + + + + background-color: lightpink; + + + QFrame::Box + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + System Management + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + QLayout::SetDefaultConstraint + + + + + + + + + Manage geometry + + + - - - - - - - Manage geometry - - - - - - - Change system type - - - - - - - Set BS channel - - - - - - - - - - - Save system config - - - - - - - Load system config - - - - - - - - - Qt::Vertical - - - - 20 - 5 - - - - - + + + Change system type + + + + + + + Set BS channel + + - - + + + + + + + Save system config + + + + + + + Load system config + + + + + + + + + + + + + + + 0 + 0 + + + + Mode + + + + + + Flying + + + true + + + + + + + Geometry + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + - + Qt::Horizontal @@ -414,12 +426,21 @@ - - - - QLayout::SetMaximumSize + + + + + + + Qt::Vertical - + + + 20 + 40 + + + diff --git a/src/cfclient/ui/widgets/geo_estimator.ui b/src/cfclient/ui/widgets/geo_estimator.ui new file mode 100644 index 000000000..67f1db79a --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator.ui @@ -0,0 +1,410 @@ + + + Form + + + + 0 + 0 + 702 + 753 + + + + + 0 + 0 + + + + Form + + + + + + + true + + + + Geometry estimator + + + + + + + + + + + + + 0 + 0 + + + + QAbstractScrollArea::AdjustToContents + + + QAbstractItemView::SelectRows + + + false + + + true + + + false + + + false + + + + + + + + + + + + Sample collection + + + + + + TextLabel + + + + + + + + + + Image + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + + 0 + 0 + + + + Start measurement + + + + + + + QFrame::Panel + + + TextLabel + + + + + + + Qt::Horizontal + + + + + + + + + + 0 + 0 + + + + Previous + + + + + + + + 0 + 0 + + + + Next + + + + + + + + + + + + + 0 + 0 + + + + Data status + + + + + + + + + 0 + 0 + + + + + + + Origin + + + false + + + + + + + + + + X-axis + + + false + + + + + + + XY-plane + + + false + + + + + + + + + + + XYZ-space + + + false + + + + + + + Verification + + + + + + + + + + + + + 0 + 0 + + + + Solution status + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + + + + Show sample details + + + + + + + Base station links + + + + + + QLayout::SetDefaultConstraint + + + 2 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Session management + + + + + + + + Load + + + + + + + Save as... + + + + + + + + + + + Restore sesssion + + + + + + + New session + + + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + diff --git a/src/cfclient/ui/wizards/bslh_1.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_1.png similarity index 100% rename from src/cfclient/ui/wizards/bslh_1.png rename to src/cfclient/ui/widgets/geo_estimator_resources/bslh_1.png diff --git a/src/cfclient/ui/wizards/bslh_2.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_2.png similarity index 100% rename from src/cfclient/ui/wizards/bslh_2.png rename to src/cfclient/ui/widgets/geo_estimator_resources/bslh_2.png diff --git a/src/cfclient/ui/wizards/bslh_3.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_3.png similarity index 100% rename from src/cfclient/ui/wizards/bslh_3.png rename to src/cfclient/ui/widgets/geo_estimator_resources/bslh_3.png diff --git a/src/cfclient/ui/wizards/bslh_4.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_4.png similarity index 100% rename from src/cfclient/ui/wizards/bslh_4.png rename to src/cfclient/ui/widgets/geo_estimator_resources/bslh_4.png diff --git a/src/cfclient/ui/wizards/bslh_5.png b/src/cfclient/ui/widgets/geo_estimator_resources/bslh_5.png similarity index 100% rename from src/cfclient/ui/wizards/bslh_5.png rename to src/cfclient/ui/widgets/geo_estimator_resources/bslh_5.png 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..6b2c82b8c --- /dev/null +++ b/src/cfclient/ui/widgets/geo_estimator_widget.py @@ -0,0 +1,771 @@ +#!/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, QGridLayout +from PyQt6.QtWidgets import QMessageBox, QPushButton, QLabel +from PyQt6.QtCore import QTimer, QAbstractTableModel, QVariant, Qt, QModelIndex + + +import logging +from enum import Enum +import threading + +import cfclient + +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry +from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader +from cflib.localization.lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader +from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors +from cflib.localization.lighthouse_types import LhDeck4SensorPositions +from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample, LhCfPoseSampleType, LhCfPoseSampleStatus +from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer, LhGeoEstimationManager +from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution +from cflib.localization.user_action_detector import UserActionDetector + +__author__ = 'Bitcraze AB' +__all__ = ['GeoEstimatorWidget'] + +logger = logging.getLogger(__name__) + +(geo_estimator_widget_class, connect_widget_base_class) = ( + uic.loadUiType(cfclient.module_path + '/ui/widgets/geo_estimator.ui')) + + +REFERENCE_DIST = 1.0 + + +class _CollectionStep(Enum): + ORIGIN = ('bslh_1.png', + 'Step 1. Origin', + 'Put the Crazyflie where you want the origin of your coordinate system.\n') + X_AXIS = ('bslh_2.png', + 'Step 2. X-axis', + 'Put the Crazyflie on the positive X-axis,' + + f' exactly {REFERENCE_DIST} meters from the origin.\n' + + 'This will be used to define the X-axis as well as scaling of the system.') + XY_PLANE = ('bslh_3.png', + 'Step 3. XY-plane', + 'Put the Crazyflie somewhere in the XY-plane, but not on the X-axis.\n' + + 'This position is used to map the the XY-plane to the floor.\n' + + 'You can sample multiple positions to get a more precise definition.') + XYZ_SPACE = ('bslh_4.png', + 'Step 4. XYZ-space', + 'Sample points in the space that you will use.\n' + + 'Make sure all the base stations are received, you need at least two base \n' + + 'stations in each sample. Sample by rotating the Crazyflie quickly \n' + + 'left-right around the Z-axis and then holding it still for a second, or \n' + + 'optionally by clicking the sample button below.\n') + + VERIFICATION = ('bslh_4.png', + 'Step 5. Verification', + 'Sample points to be used for verification of the geometry.\n' + + 'Sample by rotating the Crazyflie quickly \n' + + 'left-right around the Z-axis and then holding it still for a second, or \n' + + 'optionally by clicking the sample button below.\n') + + def __init__(self, image, title, instructions): + self.image = image + self.title = title + self.instructions = instructions + + self._order = None + + @property + def order(self): + """Get the order of the steps in the collection process""" + if self._order is None: + self._order = [self.ORIGIN, + self.X_AXIS, + self.XY_PLANE, + self.XYZ_SPACE, + 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" + + +STYLE_GREEN_BACKGROUND = "background-color: lightgreen;" +STYLE_RED_BACKGROUND = "background-color: lightpink;" +STYLE_YELLOW_BACKGROUND = "background-color: lightyellow;" +STYLE_NO_BACKGROUND = "background-color: none;" + +MOVE_COLUMN = 5 +DEL_COLUMN = 6 + + +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) + sample_selection_changed_signal = QtCore.pyqtSignal(int) + + FILE_REGEX_YAML = "Config *.yaml;;All *.*" + + def __init__(self, lighthouse_tab): + super(GeoEstimatorWidget, self).__init__() + self.setupUi(self) + + self._lighthouse_tab = lighthouse_tab + self._helper = lighthouse_tab._helper + + self._step_next_button.clicked.connect(lambda: self._change_step(self._current_step.next())) + self._step_previous_button.clicked.connect(lambda: self._change_step(self._current_step.previous())) + self._step_measure.clicked.connect(self._measure) + + self._clear_all_button.clicked.connect(self._clear_all) + self._load_button.clicked.connect(lambda: self._load_from_file(use_session_path=False)) + self._restore_button.clicked.connect(lambda: self._load_from_file(use_session_path=True)) + self._save_button.clicked.connect(self._save_to_file) + + self._timeout_reader = TimeoutAngleReader(self._helper.cf, self._timeout_reader_signal.emit) + self._timeout_reader_signal.connect(self._average_available_cb) + self._timeout_reader_result_setter = None + + self._container_updated_signal.connect(self._update_solution_info) + + self._user_notification_signal.connect(self._notify_user) + self._user_notification_clear_timer = QTimer() + self._user_notification_clear_timer.setSingleShot(True) + self._user_notification_clear_timer.timeout.connect(self._user_notification_clear) + + self._action_detector = UserActionDetector(self._helper.cf, cb=self._user_action_detected_cb) + self._matched_reader = LighthouseMatchedSweepAngleReader(self._helper.cf, self._single_sample_ready_cb, + timeout_cb=self._single_sample_timeout_cb) + + self._container = LhGeoInputContainer(LhDeck4SensorPositions.positions) + self._session_path = os.path.join(cfclient.config_path, 'lh_geo_sessions') + self._container.enable_auto_save(self._session_path) + + self._latest_solution: LighthouseGeometrySolution = LighthouseGeometrySolution([]) + self._current_step = _CollectionStep.ORIGIN + + self.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._data_status_origin.clicked.connect(lambda: self._change_step(_CollectionStep.ORIGIN)) + self._data_status_x_axis.clicked.connect(lambda: self._change_step(_CollectionStep.X_AXIS)) + self._data_status_xy_plane.clicked.connect(lambda: self._change_step(_CollectionStep.XY_PLANE)) + self._data_status_xyz_space.clicked.connect(lambda: self._change_step(_CollectionStep.XYZ_SPACE)) + self._data_status_verification.clicked.connect(lambda: self._change_step(_CollectionStep.VERIFICATION)) + + self._samples_details_model = SampleTableModel(self) + self._samples_table_view.setModel(self._samples_details_model) + self._samples_table_view.selectionModel().currentRowChanged.connect(self._selection_changed) + + header = self._samples_table_view.horizontalHeader() + header.setSectionResizeMode(0, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header.setSectionResizeMode(1, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header.setSectionResizeMode(2, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header.setSectionResizeMode(3, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + + self._sample_details_checkbox.setChecked(False) + self._samples_table_view.setVisible(False) + self._sample_details_checkbox.stateChanged.connect(self._sample_details_checkbox_state_changed) + + self._bs_linkage_handler = BsLinkageHandler(self._bs_linkage_grid) + + def _selection_changed(self, current: QModelIndex, previous: QModelIndex): + self.sample_selection_changed_signal.emit(current.row()) + + def set_selected_sample(self, index: int): + self._samples_table_view.selectRow(index) + + 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): + dlg = QMessageBox(self) + dlg.setWindowTitle("Clear samples Confirmation") + dlg.setText("Are you sure you want to clear all samples and start over?") + dlg.setStandardButtons(QMessageBox.StandardButton.Yes | QMessageBox.StandardButton.No) + button = dlg.exec() + + if button == QMessageBox.StandardButton.Yes: + self.new_session() + + def _load_from_file(self, 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_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 _sample_details_checkbox_state_changed(self, state: int): + enabled = state == Qt.CheckState.Checked.value + self._samples_table_view.setVisible(enabled) + + 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._step_title.setText(step.title) + self._step_image.setPixmap(QtGui.QPixmap( + cfclient.module_path + '/ui/widgets/geo_estimator_resources/' + step.image)) + self._step_instructions.setText(step.instructions) + self._step_info.setText('') + + if step == _CollectionStep.XYZ_SPACE: + self._step_measure.setText('Sample position') + else: + self._step_measure.setText('Start measurement') + + self._step_previous_button.setEnabled(step.has_previous()) + self._step_next_button.setEnabled(step.has_next()) + + self._update_solution_info() + + def _update_ui_reading(self, is_reading: bool): + """Update the UI to reflect whether a reading is in progress, that is enable/disable buttons""" + is_enabled = not is_reading + + self._step_measure.setEnabled(is_enabled) + self._step_next_button.setEnabled(is_enabled and self._current_step.has_next()) + self._step_previous_button.setEnabled(is_enabled and self._current_step.has_previous()) + + self._data_status_origin.setEnabled(is_enabled) + self._data_status_x_axis.setEnabled(is_enabled) + self._data_status_xy_plane.setEnabled(is_enabled) + self._data_status_xyz_space.setEnabled(is_enabled) + self._data_status_verification.setEnabled(is_enabled) + + self._load_button.setEnabled(is_enabled) + self._save_button.setEnabled(is_enabled) + self._clear_all_button.setEnabled(is_enabled) + + def _update_solution_info(self): + solution = self._latest_solution + + match self._current_step: + case _CollectionStep.ORIGIN: + self._step_solution_info.setText( + 'OK' if solution.is_origin_sample_valid else solution.origin_sample_info) + case _CollectionStep.X_AXIS: + self._step_solution_info.setText( + 'OK' if solution.is_x_axis_samples_valid else solution.x_axis_samples_info) + case _CollectionStep.XY_PLANE: + if solution.xy_plane_samples_info: + text = f'OK, {self._container.xy_plane_sample_count()} sample(s)' + else: + text = solution.xy_plane_samples_info + self._step_solution_info.setText(text) + case _CollectionStep.XYZ_SPACE: + text = f'OK, {self._container.xyz_space_sample_count()} sample(s)' + if solution.xyz_space_samples_info: + text += f', {solution.xyz_space_samples_info}' + self._step_solution_info.setText(text) + case _CollectionStep.VERIFICATION: + text = f'OK, {self._container.verification_sample_count()} sample(s)' + self._step_solution_info.setText(text) + + self._set_background_color(self._data_status_origin, solution.is_origin_sample_valid) + self._set_background_color(self._data_status_x_axis, solution.is_x_axis_samples_valid) + self._set_background_color(self._data_status_xy_plane, solution.is_xy_plane_samples_valid) + + if self._is_solving: + self._solution_status_is_ok.setText('Solving... please wait') + self._set_background_none(self._solution_status_is_ok) + else: + if solution.progress_is_ok: + self._solution_status_is_ok.setText('Solution is OK') + self._solution_status_uploaded.setText('Uploaded') + self._solution_status_max_error.setText(f'Error: {solution.error_stats.max * 1000:.1f} mm') + + verification_error = 'No data' + if solution.verification_stats: + verification_error = f'{solution.verification_stats.max * 1000:.1f} mm' + self._solution_status_verification_error.setText(f'Verification err: {verification_error}') + else: + self._solution_status_is_ok.setText('No solution') + self._solution_status_uploaded.setText('Not uploaded') + self._solution_status_max_error.setText('Error: --') + self._solution_status_verification_error.setText('Verification err: --') + self._set_background_color(self._solution_status_is_ok, solution.progress_is_ok) + + self._solution_status_info.setText(solution.general_failure_info) + + def _notify_user(self, notification_type: _UserNotificationType): + match notification_type: + case _UserNotificationType.SUCCESS: + self._helper.cf.platform.send_user_notification(True) + self._sample_collection_box.setStyleSheet(STYLE_GREEN_BACKGROUND) + self._update_ui_reading(False) + case _UserNotificationType.FAILURE: + self._helper.cf.platform.send_user_notification(False) + self._sample_collection_box.setStyleSheet(STYLE_RED_BACKGROUND) + self._update_ui_reading(False) + case _UserNotificationType.PENDING: + self._sample_collection_box.setStyleSheet(STYLE_YELLOW_BACKGROUND) + self._update_ui_reading(True) + + self._user_notification_clear_timer.stop() + self._user_notification_clear_timer.start(1000) + + def _user_notification_clear(self): + self._sample_collection_box.setStyleSheet('') + + def _set_background_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) + + 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): + self._is_solving = False + self._latest_solution = solution + self._update_solution_info() + + logger.debug('Solution ready --------------------------------------') + logger.debug(f'Converged: {solution.has_converged}') + logger.debug(f'Progress info: {solution.progress_info}') + logger.debug(f'Progress is ok: {solution.progress_is_ok}') + logger.debug(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}') + logger.debug(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}') + logger.debug(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}') + logger.debug(f'XYZ space: {solution.xyz_space_samples_info}') + logger.debug(f'General info: {solution.general_failure_info}') + + self._samples_details_model.setSolution(self._latest_solution) + + # Add action buttons to table + for row, sample in enumerate(solution.samples): + if sample.status != LhCfPoseSampleStatus.NO_DATA: + # Move button + if sample.sample_type == LhCfPoseSampleType.VERIFICATION: + button = QPushButton('To xyz') + button.clicked.connect(lambda _, uid=sample.uid: self._container.convert_to_xyz_space_sample(uid)) + else: + button = QPushButton('To verif.') + button.clicked.connect(lambda _, uid=sample.uid: self._container.convert_to_verification_sample( + uid)) + self._samples_table_view.setIndexWidget(self._samples_details_model.index(row, MOVE_COLUMN), button) + + # Delete button + button = QPushButton('Del') + button.clicked.connect(lambda _, uid=sample.uid: self._container.remove_sample(uid)) + self._samples_table_view.setIndexWidget(self._samples_details_model.index(row, DEL_COLUMN), button) + + self._bs_linkage_handler.update(solution) + + if solution.progress_is_ok: + self._upload_geometry(solution.bs_poses) + + def _upload_geometry(self, bs_poses: dict[int, LighthouseBsGeometry]): + geo_dict = {} + for bs_id, pose in bs_poses.items(): + geo = LighthouseBsGeometry() + geo.origin = pose.translation.tolist() + geo.rotation_matrix = pose.rot_matrix.tolist() + geo.valid = True + geo_dict[bs_id] = geo + + logger.info('Uploading geometry to Crazyflie') + self._lighthouse_tab.write_and_store_geometry(geo_dict) + + def _user_action_detected_cb(self): + self._measure_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) + + +class _TableRowStatus(Enum): + INVALID = 1 + LARGE_ERROR = 2 + VERIFICATION = 3 + + +class SampleTableModel(QAbstractTableModel): + def __init__(self, parent=None, *args): + QAbstractTableModel.__init__(self, parent) + self._headers = ['Type', 'X', 'Y', 'Z', 'Err', 'Move', 'Del'] + 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.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 setSolution(self, solution: LighthouseGeometrySolution): + """Set the solution and update the table values""" + self.beginResetModel() + self._table_values = [] + 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._table_highlights.append(status) + + self.endResetModel() + + +class BsLinkageHandler: + STYLE_RED_BACKGROUND = "background-color: lightpink;" + STYLE_GREEN_BACKGROUND = "background-color: lightgreen;" + + def __init__(self, container: QGridLayout): + self._container = container + + for bs in range(0, 16): + container.addWidget(self._create_label(str(bs + 1)), 0, bs) + container.addWidget(self._create_label(), 1, bs) + + def update(self, solution: LighthouseGeometrySolution): + container = self._container + link_count = solution.link_count + threshold = solution.link_count_ok_threshold + + for bs in range(0, 16): + exists = bs in link_count + count = 0 + text = '' + if exists: + count = len(link_count[bs]) + text = f'{count}' + is_ok = count >= threshold + + label_1 = container.itemAtPosition(0, bs).widget() + label_1.setVisible(exists) + + label_2 = container.itemAtPosition(1, bs).widget() + label_2.setVisible(exists) + label_2.setText(text) + if is_ok: + label_2.setStyleSheet(self.STYLE_GREEN_BACKGROUND) + else: + label_2.setStyleSheet(self.STYLE_RED_BACKGROUND) + + def _create_label(self, text=None): + label = QLabel() + label.setMinimumSize(30, 0) + label.setSizePolicy(QtWidgets.QSizePolicy.Policy.Fixed, QtWidgets.QSizePolicy.Policy.Preferred) + label.setAlignment(Qt.AlignmentFlag.AlignCenter) + + if text: + label.setText(str(text)) + else: + label.setProperty('frameShape', 'QFrame::Box') + label.setStyleSheet(STYLE_NO_BACKGROUND) + + return label 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/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())