CopterExpress/clever-show
1
0
Fork 0
mirror of https://github.com/CopterExpress/clever-show.git synced 2026-05-30 16:59:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Artem30801
2019-06-16 19:42:09 +03:00
parent a9b9694add 9790d2b164
commit adb2adedad
14 changed files with 356 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
Drone/Firmware/1.8.2-clever-v4-150519.px4
View File

File diff suppressed because one or more lines are too long

18
Drone/Firmware/px4fmu-v4-1.8.2-clever.5.px4 Normal file
View File

File diff suppressed because one or more lines are too long

94
Drone/FlightLib/FlightLib.py
View File

@@ -28,7 +28,7 @@ logger.info("Proxy services inited")
FREQUENCY = 40 # HZ
TOLERANCE = 0.2
SPEED = 1.0
SPEED_TAKEOFF = 0.8
SPEED_TAKEOFF = 1.0
TIMEOUT = 5.0
TIMEOUT_ARMED = 2.0
TIMEOUT_DESCEND = TIMEOUT
@@ -37,6 +37,7 @@ Z_DESCEND = 0.5
Z_TAKEOFF = 1.0
FRAME_ID = 'map'
INTERRUPTER = threading.Event()
FLIP_MIN_Z = 2.0
checklist = []
@@ -65,7 +66,7 @@ def check(check_name):
print(failures)
msgs = []
for failure in failures:
print(failure)
#print(failure)
msg = "[{}]: Failure: {}".format(check_name, failure)
msgs.append(msg)
logger.warning(msg)
@@ -154,9 +155,9 @@ def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, **kwargs):
set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw)
#telemetry = get_telemetry(frame_id=frame_id)
#logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
#print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
#logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))
##logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))
#print('Telemetry now: | z: {:.3f}'.format(telemetry.z))
return True
@@ -165,7 +166,7 @@ def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, **kwargs):
def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID,
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))
print('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)
# waiting for completion
@@ -176,19 +177,19 @@ 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!")
print("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(
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))
#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(
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)))
#print('Current delta: | {:.3f}'.format(
# get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z)))
time_passed = time.time() - time_start
@@ -200,14 +201,14 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO
rate.sleep()
logger.info("Point reached!")
print("Point reached!")
#print("Point reached!")
return True
def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID,
freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False):
logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
#print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
current_telem = get_telemetry(frame_id=frame_id)
navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed)
@@ -218,26 +219,26 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr
while (abs(z - telemetry.z) > tolerance) or wait:
if interrupter.is_set():
logger.warning("Reach altitude function interrupted!")
print("Reach altitude function interrupted!")
#print("Reach altitude function interrupted!")
interrupter.clear()
return
telemetry = get_telemetry(frame_id=frame_id)
logger.info('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))
#print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
# telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
time_passed = time.time() - time_start
if timeout is not None:
if time_passed >= timeout:
logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed))
print('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed))
#print('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed))
return wait
rate.sleep()
logger.info("Altitude reached!")
print("Altitude reached!")
#print("Altitude reached!")
return True
@@ -249,11 +250,11 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
timeout_descend=TIMEOUT_DESCEND, timeout_land=TIMEOUT_LAND, freq=FREQUENCY, interrupter=INTERRUPTER):
if descend:
logger.info("Descending to: | z: {:.3f}".format(z))
print("Descending to: | z: {:.3f}".format(z))
#print("Descending to: | z: {:.3f}".format(z))
reach_altitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=float('nan'), # TODO yaw
interrupter=interrupter)
landing()
print("Land is started")
#print("Land is started")
telemetry = get_telemetry(frame_id=frame_id_land)
rate = rospy.Rate(freq)
@@ -262,26 +263,26 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
while telemetry.armed:
if interrupter.is_set():
logger.warning("Land function interrupted!")
print("Land function interrupted!")
#print("Land function interrupted!")
interrupter.clear()
return False
telemetry = get_telemetry(frame_id=frame_id_land)
logger.info("Landing... | z: {}".format(telemetry.z))
print("Landing... | z: {}".format(telemetry.z))
#print("Landing... | z: {}".format(telemetry.z))
time_passed = time.time() - time_start
if timeout_land is not None:
if time_passed >= timeout_land:
logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed))
logger.warning("Disarming!")
print("Landing timed out, disarming!!!")
#print("Landing timed out, disarming!!!")
arming(False)
return False
rate.sleep()
logger.info("Landing succeeded!")
print("Landing succeeded!")
#print("Landing succeeded!")
return True
@@ -289,7 +290,7 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
timeout_arm=TIMEOUT_ARMED, timeout_takeoff=TIMEOUT, wait=False, tolerance=TOLERANCE, emergency_land=False,
interrupter=INTERRUPTER):
logger.info("Starting takeoff!")
print("Starting takeoff!")
#print("Starting takeoff!")
logger.info("Arming, going to OFFBOARD mode")
# Arming check
@@ -301,53 +302,53 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
while (not telemetry.armed) or wait:
if interrupter.is_set():
logger.warning("Takeoff function interrupted!")
print("Takeoff function interrupted!")
#print("Takeoff function interrupted!")
interrupter.clear()
return 'interrupted'
telemetry = get_telemetry()
logger.info("Arming...")
print("Arming...")
#print("Arming...")
time_passed = time.time() - time_start
if timeout_arm is not None:
if time_passed >= timeout_arm:
if not telemetry.armed:
logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed))
print('Arming timed out! | time: {:3f} seconds'.format(time_passed))
#print('Arming timed out! | time: {:3f} seconds'.format(time_passed))
return 'not armed'
else:
break
rate.sleep()
logger.info("Armed!")
print("Armed")
#print("Armed")
# Reach height
z0 = get_telemetry().z
z_dest = z + z0
navigate(z=z, speed=speed, yaw = float('nan'), auto_arm=True)
navigate(z=z, speed=speed, yaw = float('nan'), frame_id = 'body', auto_arm=True)
current_diff = abs(get_telemetry().z - z_dest)
while (current_diff > tolerance) or wait:
if interrupter.is_set():
logger.warning("Flight function interrupted!")
print("Flight function interrupted!")
#print("Flight function interrupted!")
interrupter.clear()
return 'interrupted'
current_diff = abs(get_telemetry().z - z_dest)
logger.info("Takeoff...")
print("Takeoff...")
#print("Takeoff...")
time_passed = time.time() - time_start
if timeout_takeoff is not None:
if time_passed >= timeout_takeoff:
if not wait:
logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed))
print('Takeoff timed out! | time: {:3f} seconds'.format(time_passed))
#print('Takeoff timed out! | time: {:3f} seconds'.format(time_passed))
if emergency_land:
logger.info("Preforming emergency land")
print("Preforming emergency land")
#print("Preforming emergency land")
land(descend=False, interrupter=interrupter)
return 'time out'
else:
@@ -355,5 +356,30 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
rate.sleep()
logger.info("Takeoff succeeded!")
print("Takeoff succeeded!")
#print("Takeoff succeeded!")
return 'success'
def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
logger.info("Flip started!")
start_telemetry = get_telemetry() # memorize starting position
if start_telemetry.z < min_z - TOLERANCE:
logger.warning("Can't do flip! Flip failed!")
#print("Can't do flip! Flip failed!")
else:
# Flip!
set_rates(thrust=1) # bump up
rospy.sleep(0.2)
set_rates(roll_rate=30, thrust=0.2) # maximum roll rate
while True:
telem = get_telemetry()
if -math.pi + 0.1 < telem.roll < -0.2:
break
logger.info('Flip succeeded!')
#print('Flip succeeded!')
navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw) # finish flip

8
Drone/FlightLib/LedLib.py
View File

@@ -3,6 +3,8 @@ import threading
import time
from rpi_ws281x import *
from tasking_lib import wait as wait_until
import logging
logger = logging.getLogger(__name__)
# LED strip configuration:
LED_COUNT = 60 # Number of LED pixels.
LED_PIN = 21 # GPIO pin connected to the pixels (18 uses PWM!) (10 uses SPI /dev/spidev0.0).
@@ -203,7 +205,7 @@ def strip_off():
def led_thread():
global mode
print("Starting LedLib thread")
logger.info("Starting LedLib thread")
iteration = 0
while True:
if mode == "rainbow":
@@ -240,7 +242,9 @@ def led_thread():
# init
def init_led():
def init_led(led_pin = LED_PIN):
global strip
strip = Adafruit_NeoPixel(LED_COUNT, led_pin, LED_FREQ_HZ, LED_DMA, LED_INVERT, LED_BRIGHTNESS, LED_CHANNEL)
strip.begin()
t_l = threading.Thread(target=led_thread)
t_l.daemon = True

12
Drone/client.py
View File

@@ -15,18 +15,10 @@ current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfra
parent_dir = os.path.dirname(current_dir)
sys.path.insert(0, parent_dir)
import messaging_lib as messaging
logging.basicConfig( # TODO all prints as logs
level=logging.DEBUG, # INFO
format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
handlers=[
logging.FileHandler("client_logs.log"),
logging.StreamHandler(),
])
logger = logging.getLogger(__name__)
import messaging_lib as messaging
ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"])
active_client = None # maybe needs to be refactored

3
Drone/client_config.ini
View File

@@ -15,7 +15,7 @@ port = 123
[COPTERS]
frame_id = aruco_map
takeoff_height = 1.5
takeoff_height = 2.5
takeoff_time = 8.0
safe_takeoff = False
reach_first_point_time = 8.0
@@ -26,6 +26,7 @@ y0_common = 0
[PRIVATE]
id = /hostname
use_leds = True
led_pin = 18
x0 = 0
y0 = 0

50
Drone/copter_client.py
View File

@@ -12,7 +12,16 @@ import messaging_lib as messaging
import tasking_lib as tasking
import animation_lib as animation
import ros_logging
#logging.basicConfig( # TODO all prints as logs
# level=logging.DEBUG, # INFO
# format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
# handlers=[
# logging.StreamHandler(),
# ])
logger = logging.getLogger(__name__)
#import ros_logging
class CopterClient(client.Client):
def load_config(self):
@@ -28,14 +37,14 @@ class CopterClient(client.Client):
self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common')
self.X0 = self.config.getfloat('PRIVATE', 'x0')
self.Y0 = self.config.getfloat('PRIVATE', 'y0')
self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds')
self.LED_PIN = self.config.getint('PRIVATE', 'led_pin')
def start(self, task_manager_instance):
client.logger.info("Init ROS node")
rospy.init_node('Swarm_client', anonymous=True, log_level=rospy.DEBUG)
rospy.init_node('Swarm_client', anonymous=True)
if self.USE_LEDS:
LedLib.init_led()
LedLib.init_led(self.LED_PIN)
task_manager_instance.start()
@@ -75,6 +84,31 @@ def _command_led_test(*args, **kwargs):
time.sleep(2)
LedLib.off()
@messaging.message_callback("led_fill")
def _command_emergency_led_fill(**kwargs):
r = g = b = 0
try:
r = kwargs["red"]
except KeyError:
pass
try:
g = kwargs["green"]
except KeyError:
pass
try:
b = kwargs["blue"]
except KeyError:
pass
LedLib.fill(r, g, b)
@messaging.message_callback("flip")
def _copter_flip():
FlightLib.flip()
@messaging.message_callback("takeoff")
def _command_takeoff(**kwargs):
@@ -193,8 +227,8 @@ if __name__ == "__main__":
copter_client.start(task_manager)
ros_logging.route_logger_to_ros()
ros_logging.route_logger_to_ros("__main__")
ros_logging.route_logger_to_ros("client")
ros_logging.route_logger_to_ros("messaging")
#ros_logging.route_logger_to_ros()
#ros_logging.route_logger_to_ros("__main__")
#ros_logging.route_logger_to_ros("client")
#ros_logging.route_logger_to_ros("messaging")

85
Drone/play_animation.py
View File

@@ -1,85 +0,0 @@
import time
import csv
import rospy
import logging
from FlightLib import FlightLib
from FlightLib import LedLib
logger = logging.getLogger(__name__)
animation_file_path = 'animation.csv'
USE_LEDS = True
FRAME_ID = 'aruco_map'
def takeoff(z=1.5, safe_takeoff=True, timeout=5000):
if USE_LEDS:
LedLib.wipe_to(255, 0, 0)
FlightLib.takeoff(z=z, wait=True, timeout_takeoff=timeout, emergency_land=safe_takeoff,)
LedLib.blink(0, 255, 0, wait=50)
def land(descend=False):
if USE_LEDS:
LedLib.blink(255, 0, 0)
FlightLib.land(descend=descend)
if USE_LEDS:
LedLib.off()
def animate_frame(current_frame, x0=0.0, y0=0.0, copter_frame_id=FRAME_ID):
FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'],
yaw=1.57, frame_id=copter_frame_id) # TODO yaw
if USE_LEDS:
LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue'])
def reach_frame(current_frame, x0=0.0, y0=0.0, timeout=5000, copter_frame_id=FRAME_ID):
FlightLib.reach_point(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'],
yaw=1.57, timeout=timeout, frame_id=copter_frame_id) # TODO yaw
if USE_LEDS:
LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue'])
def read_animation_file(filepath=animation_file_path):
imported_frames = []
try:
animation_file = open(filepath)
except IOError:
logging.error("File {} can't be opened".format(filepath))
else:
with animation_file:
csv_reader = csv.reader(
animation_file, delimiter=',', quotechar='|'
)
for row in csv_reader:
frame_number, x, y, z, yaw, red, green, blue = row
imported_frames.append({
'number': int(frame_number),
'x': float(x),
'y': float(y),
'z': float(z),
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
})
return imported_frames
if __name__ == '__main__':
rospy.init_node('Animation_player', anonymous=True)
if USE_LEDS:
LedLib.init_led()
X0 = 0.5
Y0 = 1.0
frames = read_animation_file()
rate = rospy.Rate(8)
takeoff()
FlightLib.reach_point(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw=11.57)
for frame in frames:
animate_frame(frame, x0=X0, y0=Y0)
rate.sleep()
land()
time.sleep(1)

6
Drone/tasking_lib.py
View File

@@ -145,9 +145,9 @@ class TaskManager(object):
except Exception as e:
logger.error("Error '{}' occurred in task {}".format(e, task))
#print("Error '{}' occurred in task {}".format(e, task))
#if str(e) == 'STOP':
# self.reset()
# return
if str(e) == 'STOP':
self.reset()
return
else:
#logger.warning("Task interrupted before execution")
#print("Task interrupted before execution")

109
Server/emergency.py
View File

@@ -1,34 +1,82 @@
# -*- coding: utf-8 -*-
# Form implementation generated from reading ui file 'emergency.ui'
#
# Created by: PyQt5 UI code generator 5.11.3
#
# WARNING! All changes made in this file will be lost!
from PyQt5 import QtCore, QtGui, QtWidgets
import os
import glob
from PyQt5 import QtWidgets
from PyQt5.QtGui import QStandardItemModel, QStandardItem
from PyQt5.QtCore import Qt, pyqtSlot
from PyQt5.QtWidgets import QFileDialog, QMessageBox
from PyQt5 import QtCore, QtGui, QtWidgets
from server import *
from PyQt5.QtCore import Qt, pyqtSlot, pyqtSignal, QObject
from PyQt5.QtWidgets import QDialog
# Importing gui form
from server_qt import *
from server import *
class Ui_Dialog(object):
def __init__(self):
self.Dialog = None
def setupUi(self, Dialog):
self.Dialog = Dialog
Dialog.setObjectName("Dialog")
Dialog.resize(632, 214)
self.pushButton_2 = QtWidgets.QPushButton(Dialog)
self.pushButton_2.setGeometry(QtCore.QRect(470, 110, 121, 61))
self.pushButton_2.setSizeIncrement(QtCore.QSize(16, 16))
self.pushButton_2.setObjectName("pushButton_2")
self.pushButton_3 = QtWidgets.QPushButton(Dialog)
self.pushButton_3.setGeometry(QtCore.QRect(40, 100, 121, 61))
self.pushButton_3.setSizeIncrement(QtCore.QSize(16, 16))
self.pushButton_3.setObjectName("pushButton_3")
self.pushButton_2.clicked.connect(self.btn_2)
self.pushButton_3.clicked.connect(self.btn_3)
Dialog.resize(746, 620)
Dialog.setStyleSheet("QDialog{\n"
"background-color: #fffdd0;\n"
"}")
self.two_button = QtWidgets.QPushButton(Dialog)
self.two_button.setGeometry(QtCore.QRect(420, 120, 231, 171))
self.two_button.setSizeIncrement(QtCore.QSize(16, 16))
self.two_button.setStyleSheet("QPushButton{\n"
"color: white;\n"
"font-weight: 600;\n"
"font-size: 25pt;\n"
"background-color: red;\n"
"}")
self.two_button.setObjectName("two_button")
self.label = QtWidgets.QLabel(Dialog)
self.label.setGeometry(QtCore.QRect(40, 40, 561, 51))
self.label.setGeometry(QtCore.QRect(90, 30, 561, 51))
font = QtGui.QFont()
font.setPointSize(16)
self.label.setFont(font)
self.label.setObjectName("label")
self.one_button = QtWidgets.QPushButton(Dialog)
self.one_button.setGeometry(QtCore.QRect(90, 120, 231, 171))
self.one_button.setSizeIncrement(QtCore.QSize(16, 16))
self.one_button.setStyleSheet("QPushButton{\n"
"color: white;\n"
"font-weight: 600;\n"
"font-size: 25pt;\n"
"background-color: RGB(118, 255, 122);\n"
"}")
self.one_button.setObjectName("one_button")
self.land_emergency_button = QtWidgets.QPushButton(Dialog)
self.land_emergency_button.setGeometry(QtCore.QRect(90, 340, 561, 81))
self.land_emergency_button.setStyleSheet("QPushButton{\n"
"font-weight: 600;\n"
"font-size: 25pt;\n"
"background-color: white;\n"
"}")
self.land_emergency_button.setObjectName("land_emergency_button")
self.disarm_emergency_button = QtWidgets.QPushButton(Dialog)
self.disarm_emergency_button.setGeometry(QtCore.QRect(90, 460, 561, 81))
self.disarm_emergency_button.setStyleSheet("QPushButton{\n"
"font-weight: 600;\n"
"font-size: 25pt;\n"
"background-color: white;\n"
"}")
self.disarm_emergency_button.setObjectName("disarm_emergency_button")
self.one_button.clicked.connect(self.one_button_click)
self.two_button.clicked.connect(self.two_button_click)
self.land_emergency_button.clicked.connect(self.land_emergency_click)
self.disarm_emergency_button.clicked.connect(self.disarm_emergency_click)
self.retranslateUi(Dialog)
QtCore.QMetaObject.connectSlotsByName(Dialog)
@@ -36,23 +84,22 @@ class Ui_Dialog(object):
def retranslateUi(self, Dialog):
_translate = QtCore.QCoreApplication.translate
Dialog.setWindowTitle(_translate("Dialog", "Dialog"))
self.pushButton_2.setText(_translate("Dialog", "PushButton"))
self.pushButton_3.setText(_translate("Dialog", "PushButton"))
self.two_button.setText(_translate("Dialog", "2"))
self.label.setText(_translate("Dialog", "\n"
"Select a group in which the drone does not work correctly"))
def btn_2(self):
for row_num in range(model.rowCount()):
item = model.item(row_num, 0)
if item.isCheckable() and item.checkState() == Qt.Checked:
copter = Client.get_by_id(item.text())
copter.send_message("green")
def btn_3(self):
for row_num in range(model.rowCount()):
item = model.item(row_num, 0)
if item.isCheckable() and item.checkState() == Qt.Checked:
copter = Client.get_by_id(item.text())
copter.send_message("red")
self.one_button.setText(_translate("Dialog", "1"))
self.land_emergency_button.setText(_translate("Dialog", "Land"))
self.disarm_emergency_button.setText(_translate("Dialog", "Disarm"))
def one_button_click(self):
self.Dialog.done(1)
def two_button_click(self):
self.Dialog.done(2)
def land_emergency_click(self):
self.Dialog.done(3)
def disarm_emergency_click(self):
self.Dialog.done(4)
if __name__ == "__main__":
import sys
app = QtWidgets.QApplication(sys.argv)

117
Server/emergency.ui
View File

@@ -6,20 +6,25 @@
<rect>
<x>0</x>
<y>0</y>
<width>632</width>
<height>214</height>
<width>746</width>
<height>620</height>
</rect>
</property>
<property name="windowTitle">
<string>Dialog</string>
</property>
<widget class="QPushButton" name="pushButton_2">
<property name="styleSheet">
<string notr="true">QDialog{
background-color: #fffdd0;
}</string>
</property>
<widget class="QPushButton" name="two_button">
<property name="geometry">
<rect>
<x>470</x>
<y>110</y>
<width>121</width>
<height>61</height>
<x>420</x>
<y>120</y>
<width>231</width>
<height>171</height>
</rect>
</property>
<property name="sizeIncrement">
@@ -28,34 +33,23 @@
<height>16</height>
</size>
</property>
<property name="text">
<string>PushButton</string>
</property>
</widget>
<widget class="QPushButton" name="pushButton_3">
<property name="geometry">
<rect>
<x>40</x>
<y>100</y>
<width>121</width>
<height>61</height>
</rect>
</property>
<property name="sizeIncrement">
<size>
<width>16</width>
<height>16</height>
</size>
<property name="styleSheet">
<string notr="true">QPushButton{
color: white;
font-weight: 600;
font-size: 25pt;
background-color: red;
}</string>
</property>
<property name="text">
<string>PushButton</string>
<string>2</string>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>40</x>
<y>40</y>
<x>90</x>
<y>30</y>
<width>561</width>
<height>51</height>
</rect>
@@ -70,6 +64,73 @@
Select a group in which the drone does not work correctly</string>
</property>
</widget>
<widget class="QPushButton" name="one_button">
<property name="geometry">
<rect>
<x>90</x>
<y>120</y>
<width>231</width>
<height>171</height>
</rect>
</property>
<property name="sizeIncrement">
<size>
<width>16</width>
<height>16</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">QPushButton{
color: white;
font-weight: 600;
font-size: 25pt;
background-color: RGB(118, 255, 122);
}</string>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QPushButton" name="land_emergency_button">
<property name="geometry">
<rect>
<x>90</x>
<y>340</y>
<width>561</width>
<height>81</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">QPushButton{
font-weight: 600;
font-size: 25pt;
background-color: white;
}</string>
</property>
<property name="text">
<string>Land</string>
</property>
</widget>
<widget class="QPushButton" name="disarm_emergency_button">
<property name="geometry">
<rect>
<x>90</x>
<y>460</y>
<width>561</width>
<height>81</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">QPushButton{
font-weight: 600;
font-size: 25pt;
background-color: white;
}</string>
</property>
<property name="text">
<string>Disarm</string>
</property>
</widget>
</widget>
<resources/>
<connections/>

67
Server/server_qt.py
View File

@@ -1,9 +1,10 @@
import os
import glob
import math
from PyQt5 import QtWidgets
from PyQt5.QtGui import QStandardItemModel, QStandardItem
from PyQt5.QtCore import Qt, pyqtSlot, QAbstractTableModel
from PyQt5.QtCore import Qt, pyqtSlot, pyqtSignal, QObject
from PyQt5.QtWidgets import QFileDialog, QMessageBox
@@ -14,12 +15,6 @@ from server import *
from copter_table_models import *
from emergency import *
class MyTableModel(QAbstractTableModel):
def __init__(self, parent, headers, *args):
QAbstractTableModel.__init__(self, parent, *args)
# noinspection PyArgumentList,PyCallByClass
class MainWindow(QtWidgets.QMainWindow):
def __init__(self):
@@ -233,17 +228,55 @@ class MainWindow(QtWidgets.QMainWindow):
copter.send_message("service_restart", {"name": "clever"})
@pyqtSlot()
def emergency(self):
for row_num in range(self.model.rowCount()):
item = self.model.item(row_num, 0)
if item.isCheckable() and item.checkState() == Qt.Checked:
copter = Client.get_by_id(item.text())
copter.send_message("emergency")
Dialog = QtWidgets.QDialog()
ui = Ui_Dialog()
ui.setupUi(Dialog)
Dialog.show()
Dialog.exec_()
client_row_min = 0
client_row_max = model.rowCount() - 1
result = -1
while (result!=0) and (result != 3) and (result != 4):
# light_green_red(min, max)
client_row_mid = int(math.ceil((client_row_max+client_row_min) / 2.0))
print(client_row_min, client_row_mid, client_row_max)
for row_num in range(client_row_min, client_row_mid):
item = model.item(row_num, 0)
copter = Client.get_by_id(item.text())
copter.send_message("led_fill", {"green": 255})
for row_num in range(client_row_mid, client_row_max + 1):
item = model.item(row_num, 0)
copter = Client.get_by_id(item.text())
copter.send_message("led_fill", {"red": 255})
Dialog = QtWidgets.QDialog()
ui = Ui_Dialog()
ui.setupUi(Dialog)
Dialog.show()
result = Dialog.exec()
print("Dialog result: {}".format(result))
if (client_row_max != client_row_min):
if (result == 1):
for row_num in range(client_row_mid, client_row_max + 1):
item = model.item(row_num, 0)
copter = Client.get_by_id(item.text())
copter.send_message("led_fill")
client_row_max = client_row_mid - 1
elif (result == 2):
for row_num in range(client_row_min, client_row_mid):
item = model.item(row_num, 0)
copter = Client.get_by_id(item.text())
copter.send_message("led_fill")
client_row_min = client_row_mid
if result == 0:
Client.broadcast_message("led_fill")
elif result == 3:
for row_num in range(client_row_min, client_row_max + 1):
item = model.item(row_num, 0)
copter = Client.get_by_id(item.text())
copter.send_message("land")
elif result == 4:
for row_num in range(client_row_min, client_row_max + 1):
item = model.item(row_num, 0)
copter = Client.get_by_id(item.text())
copter.send_message("disarm")
@pyqtSlot()
def flip(self):
reply = QMessageBox.question(

2
builder/assets/clever-show.service
View File

@@ -6,7 +6,7 @@ After=clever.service
[Service]
WorkingDirectory=/home/pi/CleverSwarm/Drone
EnvironmentFile=/lib/systemd/system/roscore.env
ExecStart=/usr/bin/python /home/pi/CleverSwarm/Drone/client.py
ExecStart=/usr/bin/python /home/pi/CleverSwarm/Drone/copter_client.py
Restart=on-failure
RestartSec=3

12
messaging_lib.py
View File

@@ -12,13 +12,13 @@ try:
except ImportError:
import selectors2 as selectors
import logging_lib
#import logging_lib
PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on",
"callback", "callback_args", "callback_kwargs",
])
_logger = logging.getLogger(__name__)
logger = logging_lib.Logger(_logger, True)
logger = logging.getLogger(__name__)
#logger = logging_lib.Logger(_logger, True)
class MessageManager:
@@ -269,8 +269,7 @@ class ConnectionManager(object):
def process_received(self, income_message):
message_type = income_message.jsonheader["message-type"]
logger.debug("Received message! Header: {}, content: {}".format(
income_message.jsonheader, income_message.content))
logger.debug("Received message! Header: {}, content: {}".format(income_message.jsonheader, income_message.content))
if message_type == "message":
self._process_message(income_message)
@@ -348,8 +347,7 @@ class ConnectionManager(object):
# Resource temporarily unavailable (errno EWOULDBLOCK)
pass
except Exception as error:
logger.warning("Attempt to send message {} to {} failed due error: {}".format(
self._send_buffer, self.addr, error))
logger.warning("Attempt to send message {} to {} failed due error: {}".format(self._send_buffer, self.addr, error))
if not self.resume_queue:
self._send_buffer = b''