diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index deae299..d0c304e 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
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index b093424..19e7220 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -2,6 +2,7 @@ import os
import time
import math
import rospy
+import datetime
import logging
from FlightLib import FlightLib
@@ -66,7 +67,7 @@ class CopterClient(client.Client):
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):
diff --git a/Server/server_gui.py b/Server/server_gui.py
index 794d524..94f9018 100644
--- a/Server/server_gui.py
+++ b/Server/server_gui.py
@@ -52,6 +52,7 @@ class Ui_MainWindow(object):
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 @@ class Ui_MainWindow(object):
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 @@ class Ui_MainWindow(object):
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 @@ class Ui_MainWindow(object):
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 @@ class Ui_MainWindow(object):
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 @@ class Ui_MainWindow(object):
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 7141df1..60e9d8e 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