mirror of
https://github.com/CopterExpress/clever-show.git
synced 2026-05-26 15:13:26 +00:00
Merge branch 'qt-gui-update' of https://github.com/CopterExpress/clever-show into qt-gui-update
This commit is contained in:
2
.gitignore
vendored
2
.gitignore
vendored
@@ -17,7 +17,7 @@ __pycache__/
|
||||
|
||||
# Development
|
||||
images/
|
||||
|
||||
show-env/
|
||||
Server/tests.py
|
||||
Server/convert_ui.sh
|
||||
Server/config/server.ini
|
||||
|
||||
@@ -53,7 +53,7 @@ def get_id(filepath="animation.csv"):
|
||||
logger.debug("No animation id in file")
|
||||
return anim_id
|
||||
|
||||
def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1):
|
||||
def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1, z_ratio=1):
|
||||
try:
|
||||
animation_file = open(filepath)
|
||||
except IOError:
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
# This is generated config_attrs with defaults
|
||||
# This is generated config with default values
|
||||
# Modify to configure
|
||||
config_name = Copter config
|
||||
config_version = 0.0
|
||||
@@ -12,14 +12,69 @@ buffer_size = 1024
|
||||
use = True
|
||||
port = 8181
|
||||
|
||||
[TELEMETRY]
|
||||
transmit = True
|
||||
frequency = 1.0
|
||||
log_resources = True
|
||||
|
||||
[VISUAL POSE WATCHDOG]
|
||||
timeout = 1.0
|
||||
pos_delta_max = 3.0
|
||||
# Available options: emergency_land, land, disarm
|
||||
action = emergency_land
|
||||
|
||||
[EMERGENCY LAND]
|
||||
thrust = 0.45
|
||||
decrease_thrust_after = 5.0
|
||||
disarm_timeout = 10.0
|
||||
|
||||
[COPTER]
|
||||
frame_id = map
|
||||
takeoff_height = 1.0
|
||||
takeoff_time = 5.0
|
||||
safe_takeoff = False
|
||||
reach_first_point_time = 5.0
|
||||
land_time = 1.0
|
||||
land_timeout = 10.0
|
||||
common_offset = 0.0, 0.0, 0.0
|
||||
|
||||
[FLOOR FRAME]
|
||||
enabled = False
|
||||
parent = map
|
||||
# Frame translation (x, y, z)
|
||||
# __list__ x y z
|
||||
translation = 0.0, 0.0, 0.0
|
||||
# Frame rotation (roll, pitch, yaw) in degrees
|
||||
# __list__ roll pitch yaw
|
||||
rotation = 0.0, 0.0, 0.0
|
||||
|
||||
[ANIMATION]
|
||||
takeoff_detection = True
|
||||
land_detection = True
|
||||
frame_delay = 0.1
|
||||
# Animation ratio (x, y, z)
|
||||
# __list__ x y z
|
||||
ratio = 1.0, 1.0, 1.0
|
||||
# Available options: 'animation', 'nan' or a number in degrees
|
||||
yaw = 180.0
|
||||
|
||||
[LED]
|
||||
use = True
|
||||
pin = 21
|
||||
count = 60
|
||||
|
||||
[PRIVATE]
|
||||
# Available options: /hostname ; /default ; /ip ; any string 63 characters length
|
||||
id = /hostname
|
||||
# Drone's individual offset (x, y, z)
|
||||
# __list__ x y z
|
||||
offset = 0.0, 0.0, 0.0
|
||||
|
||||
[SYSTEM]
|
||||
change_hostname = True
|
||||
restart_after_rename = True
|
||||
|
||||
[NTP]
|
||||
use = False
|
||||
host = ntp1.stratum2.ru
|
||||
port = 123
|
||||
|
||||
[PRIVATE]
|
||||
# avialiable options: /hostname ; /spec_default ; /ip ; any string 63 characters lengh
|
||||
id = /hostname
|
||||
# Drone's individual offset (X, Y, Z)
|
||||
# __list__ X Y Z
|
||||
offset = 0.0, 0.0, 0.0
|
||||
|
||||
@@ -10,26 +10,23 @@ buffer_size = integer(default=1024)
|
||||
use = boolean(default=True)
|
||||
port = integer(default=8181)
|
||||
|
||||
[TELEMETRY]
|
||||
transmit = boolean(default=True)
|
||||
frequency = float(default=1.0)
|
||||
log_resources = boolean(default=True)
|
||||
|
||||
[VISUAL POSE WATCHDOG]
|
||||
timeout = float(default=1.0, min=0)
|
||||
pos_delta_max = float(default=3.0, min=0)
|
||||
# Available options: emergency_land, land, disarm
|
||||
action = string(default=emergency_land)
|
||||
[EMERGENCY LAND]
|
||||
thrust = float(default=0.45, min=0, max=1)
|
||||
decrease_thrust_after = float(default=5.0, min=0) #TODO change name PLS
|
||||
disarm_timeout = float(default=10.0, min=0)
|
||||
|
||||
[FRAMES]
|
||||
[[__many__]]
|
||||
parent = string(default=aruco_map) # todo scale?
|
||||
# Frame offset (x, y, z)
|
||||
# __list__ x y z
|
||||
offset = float_list(default=list(0.0, 0.0, 0.0), min=3, max=3)
|
||||
# Frame rotation (roll, pitch, yaw)
|
||||
# __list__ roll pitch yaw
|
||||
rotation = float_list(default=list(0.0, 0.0, 0.0), min=3, max=3)
|
||||
[EMERGENCY LAND]
|
||||
thrust = float(default=0.45, min=0, max=1)
|
||||
decrease_thrust_after = float(default=5.0, min=0)
|
||||
disarm_timeout = float(default=10.0, min=0)
|
||||
|
||||
[FLIGHT]
|
||||
[COPTER]
|
||||
frame_id = string(default=map)
|
||||
takeoff_height = float(default=1.0)
|
||||
takeoff_time = float(default=5.0, min=0)
|
||||
@@ -37,19 +34,27 @@ safe_takeoff = boolean(default=False)
|
||||
reach_first_point_time = float(default=5.0)
|
||||
land_time = float(default=1.0, min=0)
|
||||
land_timeout = float(default=10.0, min=0)
|
||||
common_offset = float_list(default=list(0, 0, 0), min=3, max=3)
|
||||
|
||||
[FLOOR FRAME]
|
||||
enabled = boolean(default=False)
|
||||
parent = string(default=map)
|
||||
# Frame translation (x, y, z)
|
||||
# __list__ x y z
|
||||
translation = float_list(default=list(0.0, 0.0, 0.0), min=3, max=3)
|
||||
# Frame rotation (roll, pitch, yaw) in degrees
|
||||
# __list__ roll pitch yaw
|
||||
rotation = float_list(default=list(0.0, 0.0, 0.0), min=3, max=3)
|
||||
|
||||
[ANIMATION]
|
||||
takeoff_detection = boolean(default=True)
|
||||
land_detection = boolean(default=True)
|
||||
default_fps = float(default=10, min=0)
|
||||
# Animation scale (x, y, z)
|
||||
frame_delay = float(default=0.1, min=0.01)
|
||||
# Animation ratio (x, y, z)
|
||||
# __list__ x y z
|
||||
scale = float_list(default=list(1.0, 1.0, 1.0), min=3, max=3)
|
||||
|
||||
[TELEMETRY]
|
||||
send = boolean(default=True)
|
||||
land_pos_delta = float(default=3.0, min=0) # TODO move to watchdog + rename?
|
||||
log_resources = boolean(default=True)
|
||||
ratio = float_list(default=list(1.0, 1.0, 1.0), min=3, max=3)
|
||||
# Available options: 'animation', 'nan' or a number in degrees
|
||||
yaw = string(default=180.0)
|
||||
|
||||
[LED]
|
||||
use = boolean(default=True)
|
||||
@@ -57,16 +62,15 @@ pin = integer(default=21, min=0, max=100)
|
||||
count = integer(default=60, min=1)
|
||||
|
||||
[PRIVATE]
|
||||
# available options: /hostname ; /default ; /ip ; any string 63 characters length
|
||||
# Available options: /hostname ; /default ; /ip ; any string 63 characters length
|
||||
id = string(default=/hostname, max=63) #TODO our re check
|
||||
# Drone's individual offset (x, y, z)
|
||||
# __list__ x y z
|
||||
offset = float_list(default=list(0, 0, 0), min=3, max=3)
|
||||
yaw = float(default=180.0)
|
||||
|
||||
[SYSTEM]
|
||||
change_hostname = boolean(default=True)
|
||||
rename_restart = boolean(default=True)
|
||||
restart_after_rename = boolean(default=True)
|
||||
|
||||
[NTP]
|
||||
use = boolean(default=False)
|
||||
|
||||
@@ -3,6 +3,7 @@ import sys
|
||||
import time
|
||||
import math
|
||||
import rospy
|
||||
import numpy
|
||||
from clever import srv
|
||||
import datetime
|
||||
import logging
|
||||
@@ -72,10 +73,12 @@ flightlib_logger.addHandler(handler)
|
||||
class CopterClient(client.Client):
|
||||
def __init__(self, config_path="config/client.ini"):
|
||||
super(CopterClient, self).__init__(config_path)
|
||||
self.load_config()
|
||||
self.frames = {}
|
||||
|
||||
def load_config(self):
|
||||
super(CopterClient, self).load_config()
|
||||
#print(self.config)
|
||||
# self.FLOOR_FRAME_EXISTS = False
|
||||
# self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency')
|
||||
# self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit')
|
||||
@@ -125,11 +128,11 @@ class CopterClient(client.Client):
|
||||
def start(self, task_manager_instance):
|
||||
rospy.loginfo("Init ROS node")
|
||||
rospy.init_node('clever_show_client')
|
||||
if self.USE_LEDS:
|
||||
LedLib.init_led(self.LED_PIN)
|
||||
if self.config.led_use:
|
||||
LedLib.init_led(self.config.led_pin)
|
||||
task_manager_instance.start() # TODO move to self
|
||||
if self.FRAME_ID == "floor":
|
||||
if self.FLOOR_FRAME_EXISTS:
|
||||
if self.config.copter_frame_id == "floor":
|
||||
if self.config.floor_frame_enabled:
|
||||
self.start_floor_frame_broadcast()
|
||||
else:
|
||||
rospy.logerror("Can't make floor frame!")
|
||||
@@ -140,14 +143,14 @@ class CopterClient(client.Client):
|
||||
|
||||
def start_floor_frame_broadcast(self):
|
||||
trans = TransformStamped()
|
||||
trans.transform.translation.x = self.FLOOR_DX
|
||||
trans.transform.translation.y = self.FLOOR_DY
|
||||
trans.transform.translation.z = self.FLOOR_DZ
|
||||
trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.FLOOR_ROLL),
|
||||
math.radians(self.FLOOR_PITCH),
|
||||
math.radians(self.FLOOR_YAW)))
|
||||
trans.header.frame_id = self.FLOOR_PARENT
|
||||
trans.child_frame_id = self.FRAME_ID
|
||||
trans.transform.translation.x = self.config.floor_frame_transtation[0]
|
||||
trans.transform.translation.y = self.config.floor_frame_transtation[1]
|
||||
trans.transform.translation.z = self.config.floor_frame_transtation[2]
|
||||
trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.config.floor_frame_rotation[0]),
|
||||
math.radians(self.config.floor_frame_rotation[1]),
|
||||
math.radians(self.config.floor_frame_rotation[2])))
|
||||
trans.header.frame_id = self.config.floor_frame_parent
|
||||
trans.child_frame_id = self.config.copter_frame_id
|
||||
static_bloadcaster.sendTransform(trans)
|
||||
|
||||
|
||||
@@ -288,10 +291,9 @@ def _response_id(*args, **kwargs):
|
||||
if new_id is not None:
|
||||
old_id = client.active_client.client_id
|
||||
if new_id != old_id:
|
||||
cfg = client.ConfigOption("PRIVATE", "id", new_id)
|
||||
client.active_client.write_config(True, cfg)
|
||||
client.active_client.config.set('PRIVATE', 'id', new_id, write=True)
|
||||
if new_id != '/hostname':
|
||||
if client.active_client.RESTART_AFTER_RENAME:
|
||||
if client.active_client.system_restart_after_rename:
|
||||
hostname = client.active_client.client_id
|
||||
configure_hostname(hostname)
|
||||
configure_hosts(hostname)
|
||||
@@ -329,19 +331,13 @@ def _response_animation_id(*args, **kwargs):
|
||||
result = animation.get_id()
|
||||
if result != 'No animation':
|
||||
logger.debug("Saving corrected animation")
|
||||
frames = animation.load_animation(os.path.abspath("animation.csv"),
|
||||
x0=client.active_client.X0 + client.active_client.X0_COMMON,
|
||||
y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
|
||||
z0=client.active_client.Z0 + client.active_client.Z0_COMMON,
|
||||
x_ratio=client.active_client.X_RATIO,
|
||||
y_ratio=client.active_client.Y_RATIO,
|
||||
z_ratio=client.active_client.Z_RATIO,
|
||||
)
|
||||
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"), 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,
|
||||
check_takeoff=client.active_client.TAKEOFF_CHECK,
|
||||
check_land=client.active_client.LAND_CHECK,
|
||||
)
|
||||
check_takeoff=client.active_client.config.animation_takeoff_detection,
|
||||
check_land=client.active_client.config.animation_land_detection,
|
||||
)
|
||||
logger.debug("Start action: {}".format(start_action))
|
||||
# Save corrected animation
|
||||
animation.save_corrected_animation(corrected_frames)
|
||||
@@ -382,9 +378,9 @@ def _response_cal_status(*args, **kwargs):
|
||||
|
||||
@messaging.request_callback("position")
|
||||
def _response_position(*args, **kwargs):
|
||||
telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
|
||||
telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
|
||||
return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
|
||||
telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.FRAME_ID)
|
||||
telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.config.copter_frame_id)
|
||||
|
||||
|
||||
@messaging.request_callback("calibrate_gyro")
|
||||
@@ -416,19 +412,17 @@ def _command_test(*args, **kwargs):
|
||||
@messaging.message_callback("move_start")
|
||||
def _command_move_start_to_current_position(*args, **kwargs):
|
||||
x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
|
||||
x_ratio=client.active_client.X_RATIO,
|
||||
y_ratio=client.active_client.Y_RATIO,
|
||||
)
|
||||
*client.active_client.config.animation_ratio)
|
||||
logger.debug("x_start = {}, y_start = {}".format(x_start, y_start))
|
||||
if not math.isnan(x_start):
|
||||
telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
|
||||
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', 'x0', telem.x - x_start)
|
||||
client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
|
||||
client.active_client.rewrite_config()
|
||||
client.active_client.load_config()
|
||||
logger.info("Set start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
|
||||
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],
|
||||
client.active_client.config.private_offset[1]))
|
||||
else:
|
||||
logger.debug("Wrong telemetry")
|
||||
else:
|
||||
@@ -437,28 +431,28 @@ 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', 'x0', 0)
|
||||
client.active_client.config.set('PRIVATE', 'y0', 0)
|
||||
client.active_client.rewrite_config()
|
||||
client.active_client.load_config()
|
||||
logger.info("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
|
||||
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],
|
||||
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.FRAME_ID)
|
||||
client.active_client.config.set('PRIVATE', 'z0', telem.z)
|
||||
client.active_client.rewrite_config()
|
||||
client.active_client.load_config()
|
||||
logger.info("Set z offset to {:.2f}".format(client.active_client.Z0))
|
||||
telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
|
||||
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]))
|
||||
|
||||
|
||||
@messaging.message_callback("reset_z_offset")
|
||||
def _command_reset_z(*args, **kwargs):
|
||||
client.active_client.config.set('PRIVATE', 'z0', 0)
|
||||
client.active_client.rewrite_config()
|
||||
client.active_client.load_config()
|
||||
logger.info("Reset z offset to {:.2f}".format(client.active_client.Z0))
|
||||
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]))
|
||||
|
||||
|
||||
@messaging.message_callback("update_repo")
|
||||
@@ -491,7 +485,7 @@ def _command_service_restart(*args, **kwargs):
|
||||
|
||||
@messaging.message_callback("repair_chrony")
|
||||
def _command_chrony_repair(*args, **kwargs):
|
||||
configure_chrony_ip(client.active_client.server_host)
|
||||
configure_chrony_ip(client.active_client.config.server_host)
|
||||
restart_service("chrony")
|
||||
|
||||
|
||||
@@ -513,7 +507,7 @@ def _command_led_fill(*args, **kwargs):
|
||||
|
||||
@messaging.message_callback("flip")
|
||||
def _copter_flip(*args, **kwargs):
|
||||
FlightLib.flip(frame_id=client.active_client.FRAME_ID)
|
||||
FlightLib.flip(frame_id=client.active_client.config.copter_frame_id)
|
||||
|
||||
|
||||
@messaging.message_callback("takeoff")
|
||||
@@ -521,10 +515,10 @@ def _command_takeoff(*args, **kwargs):
|
||||
logger.info("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,
|
||||
"safe_takeoff": client.active_client.SAFE_TAKEOFF,
|
||||
"use_leds": client.active_client.USE_LEDS,
|
||||
"z": client.active_client.config.copter_takeoff_height,
|
||||
"timeout": client.active_client.config.copter_takeoff_time,
|
||||
"safe_takeoff": client.active_client.config.copter_safe_takeoff,
|
||||
"use_leds": client.active_client.config.led_use,
|
||||
}
|
||||
)
|
||||
|
||||
@@ -533,15 +527,15 @@ def _command_takeoff(*args, **kwargs):
|
||||
def _command_takeoff_z(*args, **kwargs):
|
||||
z_str = kwargs.get("z", None)
|
||||
if z_str is not None:
|
||||
telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
|
||||
telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
|
||||
logger.info("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,
|
||||
"frame_id": client.active_client.config.copter_frame_id,
|
||||
"timeout": client.active_client.config.copter_takeoff_time,
|
||||
"auto_arm": True,
|
||||
}
|
||||
)
|
||||
@@ -552,10 +546,10 @@ def _command_land(*args, **kwargs):
|
||||
task_manager.reset()
|
||||
task_manager.add_task(0, 0, animation.land,
|
||||
task_kwargs={
|
||||
"z": client.active_client.TAKEOFF_HEIGHT,
|
||||
"timeout": client.active_client.TAKEOFF_TIME,
|
||||
"frame_id": client.active_client.FRAME_ID,
|
||||
"use_leds": client.active_client.USE_LEDS,
|
||||
"z": client.active_client.config.copter_takeoff_height,
|
||||
"timeout": client.active_client.config.copter_takeoff_time,
|
||||
"frame_id": client.active_client.config.copter_frame_id,
|
||||
"use_leds": client.active_client.config.led_use,
|
||||
}
|
||||
)
|
||||
|
||||
@@ -602,44 +596,38 @@ def _play_animation(*args, **kwargs):
|
||||
|
||||
logger.info("Start time = {}, wait for {} seconds".format(start_time, start_time - time.time()))
|
||||
# Load animation
|
||||
frames = animation.load_animation(os.path.abspath("animation.csv"),
|
||||
x0=client.active_client.X0 + client.active_client.X0_COMMON,
|
||||
y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
|
||||
z0=client.active_client.Z0 + client.active_client.Z0_COMMON,
|
||||
x_ratio=client.active_client.X_RATIO,
|
||||
y_ratio=client.active_client.Y_RATIO,
|
||||
z_ratio=client.active_client.Z_RATIO,
|
||||
)
|
||||
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"), 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,
|
||||
check_takeoff=client.active_client.TAKEOFF_CHECK,
|
||||
check_land=client.active_client.LAND_CHECK,
|
||||
)
|
||||
check_takeoff=client.active_client.config.animation_takeoff_detection,
|
||||
check_land=client.active_client.config.animation_land_detection,
|
||||
)
|
||||
# Choose start action
|
||||
if start_action == 'takeoff':
|
||||
# Takeoff first
|
||||
task_manager.add_task(start_time, 0, animation.takeoff,
|
||||
task_kwargs={
|
||||
"z": client.active_client.TAKEOFF_HEIGHT,
|
||||
"timeout": client.active_client.TAKEOFF_TIME,
|
||||
"safe_takeoff": client.active_client.SAFE_TAKEOFF,
|
||||
# "frame_id": client.active_client.FRAME_ID,
|
||||
"use_leds": client.active_client.USE_LEDS,
|
||||
"z": client.active_client.config.copter_takeoff_height,
|
||||
"timeout": client.active_client.config.copter_takeoff_time,
|
||||
"safe_takeoff": client.active_client.config.copter_safe_takeoff,
|
||||
# "frame_id": client.active_client.config.copter_frame_id,
|
||||
"use_leds": client.active_client.config.led_use,
|
||||
}
|
||||
)
|
||||
# Fly to first point
|
||||
rfp_time = start_time + client.active_client.TAKEOFF_TIME
|
||||
rfp_time = start_time + client.active_client.config.copter_takeoff_time
|
||||
task_manager.add_task(rfp_time, 0, animation.execute_frame,
|
||||
task_kwargs={
|
||||
"point": animation.convert_frame(corrected_frames[0])[0],
|
||||
"color": animation.convert_frame(corrected_frames[0])[1],
|
||||
"frame_id": client.active_client.FRAME_ID,
|
||||
"use_leds": client.active_client.USE_LEDS,
|
||||
"frame_id": client.active_client.config.copter_frame_id,
|
||||
"use_leds": client.active_client.config.led_use,
|
||||
"flight_func": FlightLib.reach_point,
|
||||
}
|
||||
)
|
||||
# Calculate first frame start time
|
||||
frame_time = rfp_time + client.active_client.RFP_TIME
|
||||
frame_time = rfp_time + client.active_client.config.copter_reach_first_point_time
|
||||
|
||||
elif start_action == 'arm':
|
||||
# Calculate start time
|
||||
@@ -656,42 +644,42 @@ def _play_animation(*args, **kwargs):
|
||||
task_kwargs={
|
||||
"point": point,
|
||||
"color": color,
|
||||
"frame_id": client.active_client.FRAME_ID,
|
||||
"use_leds": client.active_client.USE_LEDS,
|
||||
"frame_id": client.active_client.config.copter_frame_id,
|
||||
"use_leds": client.active_client.config.led_use,
|
||||
"flight_func": FlightLib.navto,
|
||||
"auto_arm": True,
|
||||
}
|
||||
)
|
||||
# Calculate first frame start time
|
||||
frame_time += client.active_client.FRAME_DELAY # TODO Think about arming time
|
||||
frame_time += client.active_client.config.animation_frame_delay # TODO Think about arming time
|
||||
logger.debug(task_manager.task_queue)
|
||||
# Play animation file
|
||||
for frame in corrected_frames:
|
||||
point, color, yaw = animation.convert_frame(frame)
|
||||
if client.active_client.YAW == "animation":
|
||||
if client.active_client.config.animation_yaw == "animation":
|
||||
yaw = frame["yaw"]
|
||||
else:
|
||||
yaw = math.radians(float(client.active_client.YAW))
|
||||
yaw = math.radians(float(client.active_client.config.animation_yaw))
|
||||
task_manager.add_task(frame_time, 0, animation.execute_frame,
|
||||
task_kwargs={
|
||||
"point": point,
|
||||
"color": color,
|
||||
"yaw": yaw,
|
||||
"frame_id": client.active_client.FRAME_ID,
|
||||
"use_leds": client.active_client.USE_LEDS,
|
||||
"frame_id": client.active_client.config.copter_frame_id,
|
||||
"use_leds": client.active_client.config.led_use,
|
||||
"flight_func": FlightLib.navto,
|
||||
}
|
||||
)
|
||||
frame_time += frame["delay"]
|
||||
|
||||
# Calculate land_time
|
||||
land_time = frame_time + client.active_client.LAND_TIME
|
||||
land_time = frame_time + client.active_client.config.copter_land_time
|
||||
# Land
|
||||
task_manager.add_task(land_time, 0, animation.land,
|
||||
task_kwargs={
|
||||
"timeout": client.active_client.LAND_TIMEOUT,
|
||||
"frame_id": client.active_client.FRAME_ID,
|
||||
"use_leds": client.active_client.USE_LEDS,
|
||||
"timeout": client.active_client.config.copter_land_timeout,
|
||||
"frame_id": client.active_client.config.copter_frame_id,
|
||||
"use_leds": client.active_client.config.led_use,
|
||||
},
|
||||
)
|
||||
|
||||
@@ -743,17 +731,13 @@ class Telemetry:
|
||||
@classmethod
|
||||
def get_start_position(cls):
|
||||
x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
|
||||
x_ratio=client.active_client.X_RATIO,
|
||||
y_ratio=client.active_client.Y_RATIO,
|
||||
)
|
||||
x_delta = client.active_client.X0 + client.active_client.X0_COMMON
|
||||
y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON
|
||||
z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON
|
||||
|
||||
x = x_start + x_delta
|
||||
y = y_start + y_delta
|
||||
if not FlightLib._check_nans(x, y, z_delta):
|
||||
return x, y, z_delta
|
||||
*client.active_client.config.animation_ratio)
|
||||
offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
|
||||
x = x_start + offset[0]
|
||||
y = y_start + offset[1]
|
||||
z = offset[2]
|
||||
if not FlightLib._check_nans(x, y, z):
|
||||
return x, y, z
|
||||
return 'NO_POS'
|
||||
|
||||
@classmethod
|
||||
@@ -790,13 +774,13 @@ class Telemetry:
|
||||
def get_position(cls, ros_telemetry):
|
||||
x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z
|
||||
if not math.isnan(x):
|
||||
return x, y, z, math.degrees(ros_telemetry.yaw), client.active_client.FRAME_ID
|
||||
return x, y, z, math.degrees(ros_telemetry.yaw), client.active_client.config.copter_frame_id
|
||||
return 'NO_POS'
|
||||
|
||||
def update_telemetry_fast(self):
|
||||
self.start_position = self.get_start_position()
|
||||
try:
|
||||
self.ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
|
||||
self.ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
|
||||
if self.ros_telemetry.connected:
|
||||
self.armed = self.ros_telemetry.armed
|
||||
self.mode = self.ros_telemetry.mode
|
||||
@@ -913,10 +897,10 @@ class Telemetry:
|
||||
self.update_telemetry_fast()
|
||||
self.check_failsafe_and_interruption()
|
||||
|
||||
if client.active_client.TELEM_TRANSMIT and client.active_client.connected:
|
||||
if client.active_client.config.telemetry_transmit and client.active_client.connected:
|
||||
self.transmit_message()
|
||||
|
||||
if client.active_client.LOG_CPU_AND_MEMORY:
|
||||
if client.active_client.config.telemetry_log_resources:
|
||||
self.log_cpu_and_memory()
|
||||
|
||||
rate.sleep()
|
||||
@@ -928,9 +912,9 @@ class Telemetry:
|
||||
rate.sleep()
|
||||
|
||||
def start_loop(self):
|
||||
if client.active_client.TELEM_FREQ > 0:
|
||||
if client.active_client.config.telemetry_frequency > 0:
|
||||
telemetry_thread = threading.Thread(target=self._update_loop, name="Telemetry getting thread",
|
||||
args=(client.active_client.TELEM_FREQ,)) # TODO MOVE? Daemon?
|
||||
args=(client.active_client.config.telemetry_frequency,)) # TODO MOVE? Daemon?
|
||||
slow_telemetry_thread = threading.Thread(target=self._slow_update_loop,
|
||||
name="Slow telemetry getting thread")
|
||||
slow_telemetry_thread.start()
|
||||
|
||||
@@ -388,7 +388,9 @@ class CopterDataModel(QtCore.QAbstractTableModel):
|
||||
|
||||
def __init__(self, checks=ModelChecks, formatter=ModelFormatter, parent=None):
|
||||
super(CopterDataModel, self).__init__(parent)
|
||||
self.headers = list(columns_names.values())
|
||||
# self.headers = list(columns_names.values()) # todo
|
||||
self.headers = (' copter ID ', ' version ', ' animation ID ', ' battery ', ' system ', ' sensors ',
|
||||
' mode ', ' checks ', ' current x y z yaw frame_id ', ' start x y z ', ' dt ')
|
||||
self.data_contents = []
|
||||
|
||||
self.checks = checks
|
||||
|
||||
@@ -75,7 +75,7 @@ python3 server_qt.py
|
||||
|
||||
Для запуска анимации все коптеры должны иметь настроенную систему позиционирования. По-умолчанию в образе настроен полёт по optical flow - на коптере должен быть установлер лазерный дальномер, а камера должна быть наклонена вниз шлейфом назад.
|
||||
|
||||
Вы можете настроить один коптер на любую систему позиционирования, которую поддерживает пакет [clever](https://clever.coex.tech/ru/programming.html#positioning), а затем размножить настройки на остальные коптеры с [сервера](server.md#раздел-сервер) с помощью команды `Send launch files`.
|
||||
Вы можете настроить один коптер на любую систему позиционирования, которую поддерживает пакет [clever](https://clever.coex.tech/ru/programming.html#positioning), а затем размножить настройки на остальные коптеры с [сервера](server.md#раздел-server) с помощью команды `Send launch files`.
|
||||
|
||||
## Подготовка анимации
|
||||
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
configobj
|
||||
quamash
|
||||
configobj==5.0.6
|
||||
indexed.py==0.0.1
|
||||
numpy==1.17.4
|
||||
PyQt5==5.13.2
|
||||
PyQt5-sip==12.7.0
|
||||
numpy==1.18.1
|
||||
PyQt5==5.13.0
|
||||
PyQt5-sip==4.19.18
|
||||
Quamash==0.6.1
|
||||
selectors2==2.0.1
|
||||
Quamash==0.6.1
|
||||
six==1.13.0
|
||||
|
||||
Reference in New Issue
Block a user