From c91679964ca26349ae7726c89853a1de026fa04b Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 22 Oct 2019 14:38:52 +0300 Subject: [PATCH 1/3] Server: Add takeoff_z command --- Server/server_gui.py | 29 +++++++++++++++++++--- Server/server_gui.ui | 58 ++++++++++++++++++++++++++++++++++++++++---- Server/server_qt.py | 5 +++- 3 files changed, 82 insertions(+), 10 deletions(-) diff --git a/Server/server_gui.py b/Server/server_gui.py index 794d524e..94f90187 100644 --- a/Server/server_gui.py +++ b/Server/server_gui.py @@ -52,6 +52,7 @@ def setupUi(self, MainWindow): self.formLayout.setWidget(3, QtWidgets.QFormLayout.LabelRole, self.music_text) self.music_delay_spin = QtWidgets.QDoubleSpinBox(self.centralwidget) self.music_delay_spin.setDecimals(1) + self.music_delay_spin.setMaximum(1000.0) self.music_delay_spin.setObjectName("music_delay_spin") self.formLayout.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.music_delay_spin) self.music_checkbox = QtWidgets.QCheckBox(self.centralwidget) @@ -60,8 +61,10 @@ def setupUi(self, MainWindow): sizePolicy.setVerticalStretch(0) sizePolicy.setHeightForWidth(self.music_checkbox.sizePolicy().hasHeightForWidth()) self.music_checkbox.setSizePolicy(sizePolicy) + self.music_checkbox.setFocusPolicy(QtCore.Qt.NoFocus) + self.music_checkbox.setContextMenuPolicy(QtCore.Qt.DefaultContextMenu) self.music_checkbox.setLayoutDirection(QtCore.Qt.RightToLeft) - self.music_checkbox.setAutoFillBackground(True) + self.music_checkbox.setAutoFillBackground(False) self.music_checkbox.setText("") self.music_checkbox.setChecked(False) self.music_checkbox.setObjectName("music_checkbox") @@ -132,10 +135,10 @@ def setupUi(self, MainWindow): self.formLayout_4.setObjectName("formLayout_4") self.land_button = QtWidgets.QPushButton(self.centralwidget) self.land_button.setObjectName("land_button") - self.formLayout_4.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.land_button) + self.formLayout_4.setWidget(8, QtWidgets.QFormLayout.FieldRole, self.land_button) self.flip_button = QtWidgets.QPushButton(self.centralwidget) self.flip_button.setObjectName("flip_button") - self.formLayout_4.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.flip_button) + self.formLayout_4.setWidget(7, QtWidgets.QFormLayout.FieldRole, self.flip_button) self.takeoff_button = QtWidgets.QPushButton(self.centralwidget) self.takeoff_button.setEnabled(True) self.takeoff_button.setObjectName("takeoff_button") @@ -143,6 +146,22 @@ def setupUi(self, MainWindow): self.leds_button = QtWidgets.QPushButton(self.centralwidget) self.leds_button.setObjectName("leds_button") self.formLayout_4.setWidget(0, QtWidgets.QFormLayout.FieldRole, self.leds_button) + self.horizontalLayout_2 = QtWidgets.QHBoxLayout() + self.horizontalLayout_2.setContentsMargins(-1, 0, -1, -1) + self.horizontalLayout_2.setObjectName("horizontalLayout_2") + self.z_checkbox = QtWidgets.QCheckBox(self.centralwidget) + self.z_checkbox.setCursor(QtGui.QCursor(QtCore.Qt.ArrowCursor)) + self.z_checkbox.setFocusPolicy(QtCore.Qt.NoFocus) + self.z_checkbox.setLayoutDirection(QtCore.Qt.LeftToRight) + self.z_checkbox.setObjectName("z_checkbox") + self.horizontalLayout_2.addWidget(self.z_checkbox) + self.z_spin = QtWidgets.QDoubleSpinBox(self.centralwidget) + self.z_spin.setLayoutDirection(QtCore.Qt.LeftToRight) + self.z_spin.setDecimals(1) + self.z_spin.setProperty("value", 1.0) + self.z_spin.setObjectName("z_spin") + self.horizontalLayout_2.addWidget(self.z_spin) + self.formLayout_4.setLayout(4, QtWidgets.QFormLayout.FieldRole, self.horizontalLayout_2) self.verticalLayout.addLayout(self.formLayout_4) self.line_4 = QtWidgets.QFrame(self.centralwidget) self.line_4.setFrameShape(QtWidgets.QFrame.HLine) @@ -167,7 +186,7 @@ def setupUi(self, MainWindow): self.gridLayout.addLayout(self.horizontalLayout, 0, 0, 1, 1) MainWindow.setCentralWidget(self.centralwidget) self.menubar = QtWidgets.QMenuBar(MainWindow) - self.menubar.setGeometry(QtCore.QRect(0, 0, 1220, 26)) + self.menubar.setGeometry(QtCore.QRect(0, 0, 1220, 25)) self.menubar.setObjectName("menubar") self.menuOptions = QtWidgets.QMenu(self.menubar) self.menuOptions.setObjectName("menuOptions") @@ -278,6 +297,8 @@ def retranslateUi(self, MainWindow): self.flip_button.setText(_translate("MainWindow", "Flip")) self.takeoff_button.setText(_translate("MainWindow", "Takeoff")) self.leds_button.setText(_translate("MainWindow", "Test leds")) + self.z_checkbox.setText(_translate("MainWindow", " Z =")) + self.z_spin.setSuffix(_translate("MainWindow", " m")) self.reboot_fcu.setText(_translate("MainWindow", "Reboot FCU")) self.calibrate_gyro.setText(_translate("MainWindow", "Calibrate gyro")) self.calibrate_level.setText(_translate("MainWindow", "Calibrate level")) diff --git a/Server/server_gui.ui b/Server/server_gui.ui index 7141df17..60e9d8e6 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -91,6 +91,9 @@ 1 + + 1000.000000000000000 + @@ -101,11 +104,17 @@ 0 + + Qt::NoFocus + + + Qt::DefaultContextMenu + Qt::RightToLeft - true + false @@ -248,14 +257,14 @@ Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - + Land - + Flip @@ -279,6 +288,45 @@ + + + + 0 + + + + + ArrowCursor + + + Qt::NoFocus + + + Qt::LeftToRight + + + Z = + + + + + + + Qt::LeftToRight + + + m + + + 1 + + + 1.000000000000000 + + + + + @@ -328,7 +376,7 @@ 0 0 1220 - 26 + 25 @@ -496,7 +544,7 @@ Remove from table - + diff --git a/Server/server_qt.py b/Server/server_qt.py index 76343a7b..d08f822d 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -305,7 +305,10 @@ def disarm_all(self): def takeoff_selected(self, **kwargs): for copter in self.model.user_selected(): if takeoff_checks(copter): - copter.client.send_message("takeoff") + if self.ui.z_checkbox.isChecked(): + copter.client.send_message("takeoff_z", {"z":str(self.ui.z_spin.value())}) + else: + copter.client.send_message("takeoff") @pyqtSlot() @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?") From a87fab4369854375a609abbbe2f6bb66ee360f42 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 22 Oct 2019 14:36:44 +0100 Subject: [PATCH 2/3] FlightLib: Modify reach_point function, add auto_arm argument --- Drone/FlightLib/FlightLib.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index deae299b..d0c304e4 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -175,11 +175,11 @@ def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs return True -def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, +def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, auto_arm=False, freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False): - logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + rospy.loginfo('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) #print('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) - navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed) + navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed, auto_arm=auto_arm) # waiting for completion telemetry = get_telemetry(frame_id=frame_id) @@ -188,17 +188,17 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait: if interrupter.is_set(): - logger.warning("Reach point function interrupted!") + rospy.logwarn("Reach point function interrupted!") #print("Reach point function interrupted!") interrupter.clear() return False telemetry = get_telemetry(frame_id=frame_id) - logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + rospy.logdebug('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( # telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) - logger.info('Current delta: | {:.3f}'.format( + rospy.logdebug('Current delta: | {:.3f}'.format( get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) #print('Current delta: | {:.3f}'.format( # get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) @@ -207,12 +207,12 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO if timeout is not None: if time_passed >= timeout: - logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) - print('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) + rospy.logwarn('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) + # print('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) return wait rate.sleep() - logger.info("Point reached!") + rospy.loginfo("Point reached!") #print("Point reached!") return True From f4300f5988f8a176fcc612f1cc63327bce790d87 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 22 Oct 2019 14:37:27 +0100 Subject: [PATCH 3/3] Add takeoff_z command callback --- Drone/copter_client.py | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index b0934241..19e72206 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -2,6 +2,7 @@ import time import math import rospy +import datetime import logging from FlightLib import FlightLib @@ -66,7 +67,7 @@ def on_broadcast_bind(self): def start(self, task_manager_instance): client.logger.info("Init ROS node") - rospy.init_node('Swarm_client') + rospy.init_node('clever_show_client') if self.USE_LEDS: LedLib.init_led(self.LED_PIN) task_manager_instance.start() @@ -417,7 +418,8 @@ def _copter_flip(*args, **kwargs): @messaging.message_callback("takeoff") def _command_takeoff(*args, **kwargs): - task_manager.add_task(time.time(), 0, animation.takeoff, + print("Takeoff at {}".format(datetime.datetime.now())) + task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, "timeout": client.active_client.TAKEOFF_TIME, @@ -426,6 +428,23 @@ def _command_takeoff(*args, **kwargs): } ) +@messaging.message_callback("takeoff_z") +def _command_takeoff_z(*args, **kwargs): + z_str = kwargs.get("z", None) + if z_str is not None: + telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + print("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) + task_manager.add_task(0, 0, FlightLib.reach_point, + task_kwargs={ + "x": telem.x, + "y": telem.y, + "z": float(z_str), + "frame_id": client.active_client.FRAME_ID, + "timeout": client.active_client.TAKEOFF_TIME, + "auto_arm": True, + } + ) + @messaging.message_callback("land") def _command_land(*args, **kwargs):