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 branch 'qt-gui-update' of https://github.com/CopterExpress/clever-show into qt-gui-update

Browse Source
This commit is contained in:
Artem30801
2020-04-07 15:52:30 +03:00
parent 8d595134da 3d96569b3c
commit 9c336d8004
11 changed files with 119 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Drone/FlightLib/FlightLib.py
View File

@@ -6,7 +6,13 @@ import time
import logging
import threading
import rospy
from clever import srv
# for backward compatibility with clever
try:
from clever import srv
except ImportError:
from clover import srv
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
from std_srvs.srv import Trigger
@@ -337,7 +343,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra
return 'interrupted'
climb = abs(get_telemetry_locked(frame_id=frame_id).z - start.z)
rospy.logdebug("Takeoff to {:.2f} of {:.2f} meters".format(climb, height))
rospy.loginfo("Takeoff to {:.2f} of {:.2f} meters".format(climb, height))
time_passed = time.time() - time_start

2
Drone/client.py
View File

@@ -13,7 +13,7 @@ from contextlib import closing
import inspect # Add parent dir to PATH to import messaging_lib
current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parent_dir = os.path.dirname(current_dir)
sys.path.insert(0, parent_dir)
sys.path.insert(0, parent_dir)
logger = logging.getLogger(__name__)

4
Drone/client_setup.sh
View File

@@ -2,7 +2,7 @@
# $1 - ssid, $2 - password of wifi router
# $3 - hostname of rpi
# $4 - server ip
# $4 - server ip
if [ $(whoami) != "root" ]; then
echo -e "\nThis should be run as root!\n"
@@ -38,7 +38,7 @@ country=GB
network={
ssid="$1"
psk="$2"
scan_ssid=1
scan_ssid=1
}
EOF

30
Drone/copter_client.py
View File

@@ -4,7 +4,13 @@ import time
import math
import rospy
import numpy
from clever import srv
# for backward compatibility with clever
try:
from clever import srv
except ImportError:
from clover import srv
import datetime
import logging
import threading
@@ -13,7 +19,6 @@ import subprocess
from collections import namedtuple
from FlightLib import FlightLib
from FlightLib import LedLib
import client
@@ -69,6 +74,10 @@ flightlib_logger = logging.getLogger('FlightLib')
flightlib_logger.setLevel(logging.INFO)
flightlib_logger.addHandler(handler)
mavros_mavlink_logger = logging.getLogger('mavros_mavlink')
mavros_mavlink_logger.setLevel(logging.INFO)
mavros_mavlink_logger.addHandler(handler)
class CopterClient(client.Client):
def __init__(self, config_path="config/client.ini"):
@@ -84,8 +93,9 @@ class CopterClient(client.Client):
def start(self, task_manager_instance):
rospy.loginfo("Init ROS node")
rospy.init_node('clever_show_client')
rospy.init_node('clever_show_client', anonymous=True)
if self.config.led_use:
from FlightLib import LedLib
LedLib.init_led(self.config.led_pin)
task_manager_instance.start() # TODO move to self
if self.config.copter_frame_id == "floor":
@@ -293,7 +303,7 @@ def _response_animation_id(*args, **kwargs):
if result != 'No animation':
logger.debug("Saving corrected animation")
offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
frames = animation.load_animation(os.path.abspath("animation.csv"), client.active_client.config.animation_frame_delay,
frames = animation.load_animation(os.path.abspath("animation.csv"), client.active_client.config.animation_frame_delay,
offset[0], offset[1], offset[2], *client.active_client.config.animation_ratio)
# Correct start and land frames in animation
corrected_frames, start_action, start_delay = animation.correct_animation(frames,
@@ -380,10 +390,10 @@ def _command_move_start_to_current_position(*args, **kwargs):
telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
if not math.isnan(telem.x):
client.active_client.config.set('PRIVATE', 'offset',
client.active_client.config.set('PRIVATE', 'offset',
[telem.x - x_start, telem.y - y_start, client.active_client.config.private_offset[2]],
write=True)
logger.info("Set start delta: {:.2f} {:.2f}".format(client.active_client.config.private_offset[0],
logger.info("Set start delta: {:.2f} {:.2f}".format(client.active_client.config.private_offset[0],
client.active_client.config.private_offset[1]))
else:
logger.debug("Wrong telemetry")
@@ -393,17 +403,17 @@ def _command_move_start_to_current_position(*args, **kwargs):
@messaging.message_callback("reset_start")
def _command_reset_start(*args, **kwargs):
client.active_client.config.set('PRIVATE', 'offset',
client.active_client.config.set('PRIVATE', 'offset',
[0, 0, client.active_client.config.private_offset[2]],
write=True)
logger.info("Reset start to {:.2f} {:.2f}".format(client.active_client.config.private_offset[0],
logger.info("Reset start to {:.2f} {:.2f}".format(client.active_client.config.private_offset[0],
client.active_client.config.private_offset[1]))
@messaging.message_callback("set_z_to_ground")
def _command_set_z(*args, **kwargs):
telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
client.active_client.config.set('PRIVATE', 'offset',
client.active_client.config.set('PRIVATE', 'offset',
[client.active_client.config.private_offset[0], client.active_client.config.private_offset[1], telem.z],
write=True)
logger.info("Set z offset to {:.2f}".format(client.active_client.config.private_offset[2]))
@@ -411,7 +421,7 @@ def _command_set_z(*args, **kwargs):
@messaging.message_callback("reset_z_offset")
def _command_reset_z(*args, **kwargs):
client.active_client.config.set('PRIVATE', 'offset',
client.active_client.config.set('PRIVATE', 'offset',
[client.active_client.config.private_offset[0], client.active_client.config.private_offset[1], 0],
write=True)
logger.info("Reset z offset to {:.2f}".format(client.active_client.config.private_offset[2]))

50
Drone/mavros_mavlink.py
View File

@@ -6,6 +6,8 @@ from mavros_msgs.srv import ParamGet, ParamSet
from mavros_msgs.msg import State, ParamValue
from pymavlink.dialects.v20 import common as mavlink
logger = logging.getLogger(__name__)
send_command_long = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
get_param = rospy.ServiceProxy('/mavros/param/get', ParamGet)
set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
@@ -61,7 +63,7 @@ def calibrate(sensor):
return False
# Make calibration message
calibration_message = calibration_msg(sensor)
# Send mavlink calibration command
# Send mavlink calibration command
send_command_long(False, mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0, *calibration_message)
rospy.loginfo('Send {} calibration message'.format(sensor))
# Wait until system status to uninit (during calibration on px4)
@@ -85,7 +87,7 @@ def get_calibration_status():
if mag_status.value.integer == 0 and mag_status.success:
status_text += "mag: uncalibrated; "
if acc_status.value.integer == 0 and acc_status.success:
status_text += "acc: uncalibrated; "
status_text += "acc: uncalibrated; "
if status_text == "":
if not gyro_status.success or not mag_status.success or not acc_status.success:
status_text = "NO_INFO"
@@ -127,23 +129,45 @@ def stop_subscriber():
def load_param_file(px4_file):
result = True
err_lines = ""
err_params = ""
lines_commented = ""
params_loaded = ""
try:
px4_params = open(px4_file)
except IOError:
logging.error("File {} can't be opened".format(filepath))
logger.error("File {} can't be opened".format(filepath))
result = False
else:
else:
with open(px4_file) as px4_params:
row = 0
for line in px4_params:
param_str_array = line[:-1].split('\t')
param_name = param_str_array[2]
param_value_str = param_str_array[3]
param_type = param_str_array[4]
if param_type == '6':
param_value = ParamValue(integer=int(param_value_str))
row += 1
param_str_array = line.split('\t')
if len(param_str_array) == 5 and '#' not in param_str_array[0]:
param_name = param_str_array[2]
param_value_str = param_str_array[3]
param_type = int(param_str_array[4])
if param_type == 6:
param_value = ParamValue(integer=int(param_value_str))
else:
param_value = ParamValue(real=float(param_value_str))
if not set_param(param_name, param_value):
err_params += "{} ,".format(row)
result = False
else:
params_loaded += "{} ,".format(row)
elif '#' in param_str_array[0]:
lines_commented += "{} ,".format(row)
else:
param_value = ParamValue(real=float(param_value_str))
if not set_param(param_name, param_value):
result = False
err_lines += "{} ,".format(row)
if err_lines:
logger.info("Can't parse lines: {}".format(err_lines[:-1]))
if err_params:
logger.info("Can't set params from lines: {}".format(err_params[:-1]))
if lines_commented:
logger.info("Lines commented: {}".format(lines_commented[:-1]))
if params_loaded:
logger.info("Params are successfully loaded from lines: {}".format(params_loaded[:-1]))
return result

2
Drone/requirements.txt Normal file
View File

@@ -0,0 +1,2 @@
selectors2
psutil

20
Drone/tasking_lib.py
View File

@@ -54,7 +54,7 @@ class TaskManager(object):
self._wait_interrupt_event.set()
self._running_event.clear()
task = Task(task_function, task_args, task_kwargs, task_delayable)
task = Task(task_function, task_args, task_kwargs, task_delayable)
count = next(self._counter)
entry = (timestamp, priority, count, task)
@@ -64,21 +64,21 @@ class TaskManager(object):
entry_old = self.task_queue[0]
else:
entry_old = entry
heapq.heappush(self.task_queue, entry)
if self.task_queue[0] != entry_old:
self._task_interrupt_event.set()
#print("Task queue updated with more priority task")
if self._reset_event.is_set():
self._task_interrupt_event.set()
self._reset_event.clear()
#print("Task queue updated after reset")
self._wait_interrupt_event.clear()
self._running_event.set()
# #print(self.task_queue)
def pop_task(self):
@@ -89,7 +89,7 @@ class TaskManager(object):
def get_last_task_name(self):
return self._last_task
def get_current_task(self):
try:
start_time, priority, count, task = self.task_queue[0]
@@ -134,7 +134,7 @@ class TaskManager(object):
if self.task_queue:
next_task_time = self.task_queue[0][0]
if time_to_start_next_task > next_task_time:
self._timeshift = time_to_start_next_task - next_task_time
self._timeshift = time_to_start_next_task - next_task_time
self._wait_interrupt_event.clear()
self._task_interrupt_event.clear()
self._running_event.set()
@@ -178,7 +178,7 @@ class TaskManager(object):
#print("Interrupter is set: {}".format(self._task_interrupt_event.is_set()))
try:
task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs)
except Exception as e:
logger.error("Error '{}' occurred in task {}".format(e, task))
#print("Error '{}' occurred in task {}".format(e, task))
@@ -211,10 +211,10 @@ class TaskManager(object):
#try:
#print("Pop {} function!".format(task.func.__name__))
#except Exception as e:
#print("Pop something!")
#print("Pop something!")
if self._task_interrupt_event.is_set():
self._task_interrupt_event.clear()
self._task_interrupt_event.clear()
logger.info("Execution done")
#print("Execution done")

28
Drone/visual_pose_watchdog.py
View File

@@ -5,7 +5,13 @@ import time
import math
import logging
import threading
from clever.srv import SetAttitude
# for backward compatibility with clever
try:
from clever import SetAttitude
except ImportError:
from clover import SetAttitude
from sensor_msgs.msg import Range
from mavros_msgs.msg import State, PositionTarget
from mavros_msgs.srv import SetMode, CommandBool
@@ -16,7 +22,7 @@ from geometry_msgs.msg import PoseStamped
import inspect # Add parent dir to PATH to import messaging_lib
current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parent_dir = os.path.dirname(current_dir)
sys.path.insert(0, parent_dir)
sys.path.insert(0, parent_dir)
from config import ConfigManager
@@ -138,7 +144,7 @@ def laser_callback(data):
laser_range = data.range
def emergency_land(disarm_if_timeout = True):
global emergency_land_thrust, laser_range
global emergency_land_thrust, laser_range
current_thrust = emergency_land_thrust
action_timestamp = time.time()
while armed:
@@ -147,13 +153,13 @@ def emergency_land(disarm_if_timeout = True):
try:
set_attitude(thrust = current_thrust, yaw = 0, frame_id = 'body', auto_arm = True)
except rospy.ServiceException as e:
logger.info(e)
logger.info(e)
delta = time.time() - action_timestamp
if delta > timeout_to_disarm and disarm_if_timeout:
try:
arming(False)
except rospy.ServiceException as e:
logger.info(e)
logger.info(e)
if (laser_range < 0.1 or delta > emergency_land_decrease_thrust_after) and current_thrust >= 0.:
current_thrust -= 0.02
if current_thrust <= 0.03:
@@ -161,7 +167,7 @@ def emergency_land(disarm_if_timeout = True):
try:
arming(False)
except rospy.ServiceException as e:
logger.info(e)
logger.info(e)
rate.sleep()
def emergency_land_service(request):
@@ -175,7 +181,7 @@ def emergency_land_service(request):
responce.success = False
responce.message = "Copter is disarmed, no need for emergency landing!"
emergency_land_called = False
return responce
return responce
def watchdog_callback(event):
global visual_pose_last_timestamp, armed, mode, watchdog_action, laser_range
@@ -218,7 +224,7 @@ def watchdog_callback(event):
try:
arming(False)
except rospy.ServiceException as e:
logger.info(e)
logger.info(e)
rate.sleep()
elif watchdog_action == 'disarm':
logger.info('Visual pose data is too old, copter is armed, disarming...')
@@ -226,15 +232,15 @@ def watchdog_callback(event):
try:
arming(False)
except rospy.ServiceException as e:
logger.info(e)
rate.sleep()
logger.info(e)
rate.sleep()
elif watchdog_action == 'emergency_land':
if visual_pose_dt > visual_pose_timeout:
logger.info('Visual pose data is too old, copter is armed, emergency landing...')
if pos_delta > pos_delta_max:
logger.info('Position delta is {} m, copter is armed, emergency landing...'.format(pos_delta))
emergency_land()
logger.info('Disarmed')
logger.info('Disarmed')
emergency = False
if emergency_land_called:
emergency = True

3
Server/config/spec/configspec_server.ini
View File

@@ -5,6 +5,9 @@ config_version = float(default='1.0')
port = integer(default=25000)
buffer_size = integer(default=1024)
[CLIENT]
clever_dir = string(default=/home/pi/catkin_ws/src/clever/clover)
[TABLE]
# True -> clients are removed on disconnection
# False -> disconnected clients indicated

17
Server/server_qt.py
View File

@@ -310,16 +310,17 @@ class MainWindow(QtWidgets.QMainWindow):
@confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?")
def send_start_time_selected(self):
time_now = server.time_now()
time_lag = 0.1
dt = self.ui.start_delay_spin.value()
logging.info('Wait {} seconds to start animation'.format(dt))
if self.ui.music_checkbox.isChecked():
music_dt = self.ui.music_delay_spin.value()
asyncio.ensure_future(self.play_music_at_time(music_dt + time_now), loop=loop)
asyncio.ensure_future(self.play_music_at_time(music_dt + time_now + time_lag), loop=loop)
logging.info('Wait {} seconds to play music'.format(music_dt))
# This filter constraints takeoff in real world, when copter state was normal and then some checks were failed for a while
# for copter in filter(lambda copter: copter.states.all_checks, self.model.user_selected()):
for copter in self.model.user_selected():
server.send_starttime(copter.client, dt + time_now)
server.send_starttime(copter.client, dt + time_now + time_lag)
@pyqtSlot()
def pause_resume_selected(self):
@@ -438,7 +439,7 @@ class MainWindow(QtWidgets.QMainWindow):
def request_any_file(self, client_path=None, copters=None):
if client_path is None:
_client_path, ok = QInputDialog.getText(self, "Enter path of file to request from client", "Source:",
QLineEdit.Normal, "/home/pi/")
QLineEdit.Normal, "")
if not ok:
return
client_path = _client_path
@@ -467,7 +468,7 @@ class MainWindow(QtWidgets.QMainWindow):
return
c_path, ok = QInputDialog.getText(self, "Enter path (and name) to send on client", "Destination:",
QLineEdit.Normal, "/home/pi/") # TODO config?
QLineEdit.Normal, "") # TODO config?
if not ok:
return
@@ -483,7 +484,7 @@ class MainWindow(QtWidgets.QMainWindow):
@pyqtSlot()
def send_calibrations(self):
self.send_directory_files("Select directory with calibrations", ('.yaml', ), match_id=True,
client_path="/home/pi/catkin_ws/src/clever/clever/camera_info/",
client_path=os.path.join(self.server.config.client_clever_dir,"camera_info/"),
client_filename="calibration.yaml") # TODO callback to reload clever?
# from os.path import expanduser # TODO on client
@@ -495,13 +496,13 @@ class MainWindow(QtWidgets.QMainWindow):
copter.client.send_message("service_restart", kwargs={"name": "clever"})
self.send_files("Select aruco map configuration file", "Aruco map files (*.txt)", onefile=True,
client_path="/home/pi/catkin_ws/src/clever/aruco_pose/map/",
client_path=os.path.abspath(os.path.join(self.server.config.client_clever_dir,"../aruco_pose/map/")),
client_filename="animation_map.txt", callback=callback)
@pyqtSlot()
def send_launch(self):
self.send_directory_files("Select directory with launch files", ('.launch', '.yaml'), match_id=False,
client_path='/home/pi/catkin_ws/src/clever/clever/launch/') # TODO clever restart callback?
client_path=os.path.join(self.server.config.client_clever_dir,"launch/")) # TODO clever restart callback?
@pyqtSlot()
def send_fcu_parameters(self):
@@ -652,7 +653,7 @@ if __name__ == "__main__":
logging.StreamHandler(),
msgbox_handler
])
sys.excepthook = except_hook # for debugging (exceptions traceback)
app = QApplication(sys.argv)

68
docs/ru/server.md
View File

@@ -7,8 +7,6 @@
* [Настройка](#настройка-сервера)
* [Дополнительные операции](#дополнительные-операции)
[TOC]
## Интерфейс сервера
Сервер имеет визуальный графический интерфейс для удобства взаимодействия.
@@ -17,7 +15,11 @@
### Таблица состояния коптеров
При первом подключении клиента к серверу в таблицу добавляется строка для отображения состояния клиента, содержащая только имя клиента (`copter ID`). Если на клиентах настроена автоматическая передача телеметрии, данные в таблице будут обновляться автоматически. Так же возможно запросить телеметрию выбранных клиентов с помощью кнопки [`Preflight check`](#управление) Строки можно сортировать по возрастанию или убыванию значений любого из столбцов, кликнув по его заголовку.
При первом подключении клиента к серверу в таблицу добавляется строка для отображения состояния клиента, содержащая только имя клиента (`copter ID`). Если на клиентах настроена автоматическая передача телеметрии, данные в таблице будут обновляться автоматически. Так же возможно запросить телеметрию выбранных клиентов с помощью кнопки [`Preflight check`](#управление).
Строки можно сортировать по возрастанию или убыванию значений любого из столбцов, кликнув по его заголовку.
Столбцы можно менять местами и изменять их ширину: все изменения сохраняются в настройках сервера.
Ячейки таблицы подсвечиваются:
@@ -177,63 +179,13 @@
### Файл конфигурации
Конфигурация сервера задаётся в файле [server.ini](../../Server/config/server.ini), имеющем следующий вид по умолчанию:
Конфигурация сервера создаётся согласно [спецификации](../../Server/config/spec/configspec_server.ini), в ней можно посмотреть значения по умолчанию для любого параметра после ключевого слова `default`. Все изменения сохраняются в файл конфигурации `server.ini` в папке `clever-show/Server/config`.
```ini
# This is generated config with default values
# Modify to configure
config_name = server
config_version = 1.0
Доступно редактирование конфигурации сервера через GUI модуль `Config editor` через меню `Server -> Edit server config`.
[SERVER]
port = 25000
buffer_size = 1024
Конфигурация по умолчанию является полностью работоспособной и не требует изменений для быстрого старта клиента.
[TABLE]
# True -> clients are removed on disconnection
# False -> disconnected clients indicated
remove_disconnected = False
[[PRESETS]]
current = DEFAULT
[[[DEFAULT]]]
copter_id = True, 100
git_version = True, 75
config_version = True, 140
animation_id = True, 100
battery = True, 100
fcu_status = True, 100
calibration_status = True, 65
mode = True, 100
selfcheck = True, 65
current_position = True, 250
start_position = True, 150
last_task = True, 250
time_delta = True, 241
[CHECKS]
check_git_version = True
check_current_position = True
# in percents; set 0 to disable this check
battery_min = 50.0
# in meters; set 0 to disable this check
start_pos_delta_max = 1.0
# in seconds
time_delta_max = 1.0
[BROADCAST]
send = True
listen = True
port = 8181
# delay for message sending in seconds
delay = 5.0
[NTP]
use = False
host = ntp1.stratum2.ru
port = 123
```
Данный файл конфигурации автоматически генерируется при первом запуске сервера, если отсутствует файл конфигурации. Пользовательский файл может содержать неполный набор параметров - в этом случае будут использоваться значения по умолчанию для отсутствующих параметров. Конфигурация по умолчанию является полностью работоспособной и не требует изменений для быстрого начала работы системы.
### Описание параметров
#### Корневой раздел
@@ -309,4 +261,4 @@ config_version = 1.0
### Column preset editor
...
...