diff --git a/Drone/FCU/clever_failsafe_and_power.params b/Drone/FCU/clever_failsafe_and_power.params new file mode 100644 index 0000000..5f95317 --- /dev/null +++ b/Drone/FCU/clever_failsafe_and_power.params @@ -0,0 +1,5 @@ +1 1 COM_LOW_BAT_ACT 2 6 +1 1 COM_OBL_ACT 0 6 +1 1 COM_OBL_RC_ACT 4 6 +1 1 BAT_V_CHARGED 4.050000190734863281 9 +1 1 BAT_V_EMPTY 3.500000000000000000 9 diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index d0c304e..abf8e59 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -22,7 +22,8 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) landing = rospy.ServiceProxy('/land', Trigger) -services_list = [navigate, set_position, set_rates, set_mode, get_telemetry, arming,landing] +services_list = ['/navigate', '/set_position', '/set_rates', '/mavros/set_mode', + '/get_telemetry', '/mavros/cmd/arming', '/land', '/mavros/param/get'] logger.info("Proxy services inited") @@ -42,7 +43,11 @@ INTERRUPTER = threading.Event() FLIP_MIN_Z = 2.0 checklist = [] +get_telemetry_lock = threading.Lock() +def get_telemetry_locked(*args, **kwargs): + with get_telemetry_lock: + return get_telemetry(*args, **kwargs) def arming_wrapper(state=False, *args, **kwargs): arming(state) @@ -76,7 +81,7 @@ def check(check_name): if msgs: return msgs else: - logger.info("[{}]: OK".format(check_name)) + logger.debug("[{}]: OK".format(check_name)) return None checklist.append(wrapper) @@ -88,27 +93,34 @@ def check(check_name): def _check_nans(*values): return any(math.isnan(x) for x in values) +def check_ros_services_unavailable(): + unavailable_services = [] + for service in services_list: + try: + rospy.wait_for_service(service, timeout=0.1) + except (rospy.ServiceException, rospy.ROSException): + unavailable_services.append(service) + return unavailable_services @check("Ros services") def check_ros_services(): - timeout = 5.0 for service in services_list: try: - service.wait_for_service(timeout=timeout) - except (rospy.ServiceException, rospy.ROSException) as e: - yield ("ROS service {} is not available!".format(service.name)) + rospy.wait_for_service(service, timeout=0.1) + except (rospy.ServiceException, rospy.ROSException): + yield ("ROS service {} is not available!".format(service)) @check("FCU connection") def check_connection(): - telemetry = get_telemetry() + telemetry = get_telemetry_locked() if not telemetry.connected: yield ("Flight controller is not connected!") @check("Linear velocity estimation") def check_linear_speeds(speed_limit=0.15): - telemetry = get_telemetry(frame_id='body') + telemetry = get_telemetry_locked(frame_id='body') if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz): yield ("Velocity estimation is not available") @@ -123,7 +135,7 @@ def check_linear_speeds(speed_limit=0.15): @check("Angular velocity estimation") def check_angular_speeds(rate_limit=0.05): - telemetry = get_telemetry(frame_id='body') + telemetry = get_telemetry_locked(frame_id='body') if _check_nans(telemetry.pitch_rate, telemetry.roll_rate, telemetry.yaw_rate): yield ("Angular velocities estimation is not available") @@ -138,7 +150,7 @@ def check_angular_speeds(rate_limit=0.05): @check("Angles estimation") def check_angles(angle_limit=math.radians(5)): - telemetry = get_telemetry(frame_id='body') + telemetry = get_telemetry_locked(frame_id='body') if _check_nans(telemetry.pitch, telemetry.roll, telemetry.yaw): yield ("Angular velocities estimation is not available") @@ -165,7 +177,7 @@ def selfcheck(): def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs): set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, auto_arm=auto_arm) - #telemetry = get_telemetry(frame_id=frame_id) + #telemetry = get_telemetry_locked(frame_id=frame_id) 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)) @@ -182,7 +194,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO 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) + telemetry = get_telemetry_locked(frame_id=frame_id) rate = rospy.Rate(freq) time_start = time.time() @@ -193,7 +205,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO interrupter.clear() return False - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry_locked(frame_id=frame_id) 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( @@ -221,10 +233,10 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr 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)) - current_telem = get_telemetry(frame_id=frame_id) + current_telem = get_telemetry_locked(frame_id=frame_id) navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed) - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry_locked(frame_id=frame_id) rate = rospy.Rate(freq) time_start = time.time() @@ -235,7 +247,7 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr interrupter.clear() return - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry_locked(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( @@ -268,7 +280,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA landing() #print("Land is started") - telemetry = get_telemetry(frame_id=frame_id_land) + telemetry = get_telemetry_locked(frame_id=frame_id_land) rate = rospy.Rate(freq) time_start = time.time() @@ -279,7 +291,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA interrupter.clear() return False - telemetry = get_telemetry(frame_id=frame_id_land) + telemetry = get_telemetry_locked(frame_id=frame_id_land) logger.info("Landing... | z: {}".format(telemetry.z)) #print("Landing... | z: {}".format(telemetry.z)) time_passed = time.time() - time_start @@ -300,7 +312,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, timeout_takeoff=TIMEOUT, interrupter=INTERRUPTER, emergency_land=False): rospy.loginfo("Takeoff started...") rate = rospy.Rate(FREQUENCY) - start = get_telemetry(frame_id=frame_id) + start = get_telemetry_locked(frame_id=frame_id) climb = 0. time_start = time.time() result = navigate(z=height, speed=speed, yaw=float('nan'), frame_id="body", auto_arm=True) @@ -314,7 +326,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra interrupter.clear() return 'interrupted' - climb = abs(get_telemetry(frame_id=frame_id).z - start.z) + climb = abs(get_telemetry_locked(frame_id=frame_id).z - start.z) rospy.logdebug("Takeoff to {:.2f} of {:.2f} meters".format(climb, height)) time_passed = time.time() - time_start @@ -333,7 +345,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different directions logger.info("Flip started!") - start_telemetry = get_telemetry(frame_id=frame_id) # memorize starting position + start_telemetry = get_telemetry_locked(frame_id=frame_id) # memorize starting position if start_telemetry.z < min_z - TOLERANCE: logger.warning("Can't do flip! Flip failed!") @@ -347,7 +359,7 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc set_rates(roll_rate=30, thrust=0.2) # maximum roll rate while True: - telem = get_telemetry() + telem = get_telemetry_locked() if abs(telem.roll) > math.pi/2: break diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 4c12b92..f758207 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -24,7 +24,7 @@ def get_id(filepath="animation.csv"): try: animation_file = open(filepath) except IOError: - logging.error("File {} can't be opened".format(filepath)) + logger.error("File {} can't be opened".format(filepath)) anim_id = "No animation" return anim_id else: @@ -35,11 +35,45 @@ def get_id(filepath="animation.csv"): row_0 = csv_reader.next() if len(row_0) == 1: anim_id = row_0[0] - print("Got animation_id: {}".format(anim_id)) + logger.debug("Got animation_id: {}".format(anim_id)) else: - print("No animation id in file") + anim_id = "Empty id" + logger.debug("No animation id in file") return anim_id +def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1): + try: + animation_file = open(filepath) + except IOError: + logger.error("File {} can't be opened".format(filepath)) + anim_id = "No animation" + return float('nan'), float('nan') + else: + with animation_file: + csv_reader = csv.reader( + animation_file, delimiter=',', quotechar='|' + ) + try: + row_0 = csv_reader.next() + except: + return float('nan'), float('nan') + if len(row_0) == 1: + anim_id = row_0[0] + logger.debug("Got animation_id: {}".format(anim_id)) + try: + frame_number, x, y, z, yaw, red, green, blue = csv_reader.next() + except: + return float('nan'), float('nan') + else: + anim_id = "Empty id" + logger.debug("No animation id in file") + try: + frame_number, x, y, z, yaw, red, green, blue = row_0 + except: + return float('nan'), float('nan') + return float(x)*x_ratio, float(y)*y_ratio + + def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_ratio=1, z_ratio=1): imported_frames = [] global anim_id @@ -56,9 +90,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati row_0 = csv_reader.next() if len(row_0) == 1: anim_id = row_0[0] - print("Got animation_id: {}".format(anim_id)) + logger.debug("Got animation_id: {}".format(anim_id)) else: - print("No animation id in file") + logger.debug("No animation id in file") frame_number, x, y, z, yaw, red, green, blue = row_0 imported_frames.append({ 'number': int(frame_number), diff --git a/Drone/client.py b/Drone/client.py index f557081..a52e704 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -17,7 +17,6 @@ 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) -logging.basicConfig(level=logging.DEBUG) logger = logging.getLogger(__name__) import messaging_lib as messaging @@ -31,7 +30,7 @@ class Client(object): self.selector = selectors.DefaultSelector() self.client_socket = None - self.server_connection = messaging.ConnectionManager() + self.server_connection = messaging.ConnectionManager("pi") self.server_host = None self.server_port = None diff --git a/Drone/client_config.ini b/Drone/client_config.ini index 45585ce..bd58322 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -13,6 +13,10 @@ use_ntp = False host = ntp1.stratum2.ru port = 123 +[TELEMETRY] +frequency = 1 +transmit = True + [ANIMATION] takeoff_animation_check = True land_animation_check = True @@ -22,7 +26,11 @@ y_ratio = 1.0 z_ratio = 1.0 [COPTERS] +<<<<<<< HEAD frame_id = floor +======= +frame_id = map +>>>>>>> master takeoff_height = 1.0 takeoff_time = 5.0 safe_takeoff = False diff --git a/Drone/copter_client.py b/Drone/copter_client.py index cd2f113..bdf6371 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -1,9 +1,15 @@ import os +import sys import time import math import rospy +from clever import srv import datetime import logging +import threading +import subprocess +import ConfigParser +from collections import namedtuple from FlightLib import FlightLib from FlightLib import LedLib @@ -21,22 +27,54 @@ from tf.transformations import quaternion_from_euler, euler_from_quaternion, qua import tf2_ros static_bloadcaster = tf2_ros.StaticTransformBroadcaster() +Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position") +telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS') -# 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(), -# ]) +# get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) + +logging.basicConfig( # TODO all prints as logs + level=logging.DEBUG, # INFO + stream=sys.stdout, + format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", + handlers=[ + logging.StreamHandler(sys.stdout), + ]) + +handler = logging.StreamHandler(sys.stdout) +handler.setLevel(logging.DEBUG) +formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s") +handler.setFormatter(formatter) logger = logging.getLogger(__name__) +logger.setLevel(logging.DEBUG) +logger.addHandler(handler) +animation_logger = logging.getLogger('animation_lib') +animation_logger.setLevel(logging.INFO) +animation_logger.addHandler(handler) -# import ros_logging +client_logger = logging.getLogger('client') +client_logger.setLevel(logging.DEBUG) +client_logger.addHandler(handler) + +messaging_logger = logging.getLogger('messaging_lib') +messaging_logger.setLevel(logging.INFO) +messaging_logger.addHandler(handler) + +tasking_logger = logging.getLogger('tasking_lib') +tasking_logger.setLevel(logging.INFO) +tasking_logger.addHandler(handler) + +flightlib_logger = logging.getLogger('FlightLib') +flightlib_logger.setLevel(logging.INFO) +flightlib_logger.addHandler(handler) class CopterClient(client.Client): def load_config(self): + self.FLOOR_FRAME_EXISTS = False super(CopterClient, self).load_config() + self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency') + self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit') self.FRAME_ID = self.config.get('COPTERS', 'frame_id') self.FRAME_FLIPPED_HEIGHT = 0. self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height') @@ -58,7 +96,18 @@ class CopterClient(client.Client): self.Z0 = self.config.getfloat('PRIVATE', 'z0') self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') self.LED_PIN = self.config.getint('PRIVATE', 'led_pin') - + try: + self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x') + self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y') + self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z') + self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll') + self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch') + self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw') + self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent') + self.FLOOR_FRAME_EXISTS = True + except ConfigParser.Error: + rospy.logerror("No floor frame!") + self.FLOOR_FRAME_EXISTS = False self.RESTART_DHCPCD = self.config.getboolean('PRIVATE', 'restart_dhcpcd') def on_broadcast_bind(self): @@ -66,38 +115,31 @@ class CopterClient(client.Client): restart_service("chrony") def start(self, task_manager_instance): - client.logger.info("Init ROS node") + rospy.loginfo("Init ROS node") rospy.init_node('clever_show_client') if self.USE_LEDS: LedLib.init_led(self.LED_PIN) task_manager_instance.start() if self.FRAME_ID == "floor": - try: - self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x') - self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y') - self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z') - self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll') - self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch') - self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw') - self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent') - except Exception as e: - raise Exception("Can't make floor frame!") - quit() + if self.FLOOR_FRAME_EXISTS: + self.start_floor_frame_broadcast() else: - 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 - static_bloadcaster.sendTransform(trans) + rospy.logerror("Can't make floor frame!") start_subscriber() - # print(check_state_topic()) + telemetry_thread.start() super(CopterClient, self).start() + 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 + static_bloadcaster.sendTransform(trans) def restart_service(name): os.system("systemctl restart {}".format(name)) @@ -110,7 +152,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False content = raw_content.split(" ") @@ -118,11 +160,11 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): try: current_ip = content[ip_index] except IndexError: - print("Something wrong with config") + logger.error("Something wrong with config") return False if "." not in current_ip: - print("That's not ip!") + logger.debug("That's not ip!") return False if current_ip != ip: @@ -132,7 +174,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): with open(path, 'w') as f: f.write(" ".join(content)) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -144,7 +186,7 @@ def configure_hostname(hostname): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False current_hostname = str(raw_content) @@ -155,7 +197,7 @@ def configure_hostname(hostname): with open(path, 'w') as f: f.write(content) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -167,7 +209,7 @@ def configure_hosts(hostname): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False index_start = raw_content.find("127.0.1.1", ) @@ -180,7 +222,7 @@ def configure_hosts(hostname): with open(path, 'w') as f: f.write(content) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -195,7 +237,7 @@ def configure_bashrc(hostname): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False index_start = raw_content.find("ROS_HOSTNAME='", ) + 14 @@ -208,7 +250,7 @@ def configure_bashrc(hostname): with open(path, 'w') as f: f.write(content) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -253,13 +295,17 @@ def _response_selfcheck(*args, **kwargs): stop_subscriber() return "NOT_CONNECTED_TO_FCU" +@messaging.request_callback("telemetry") +def _response_telemetry(*args, **kwargs): + return create_telemetry_message(telemetry) + @messaging.request_callback("anim_id") def _response_animation_id(*args, **kwargs): # Load animation result = animation.get_id() if result != 'No animation': - print ("Saving corrected 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, @@ -273,7 +319,7 @@ def _response_animation_id(*args, **kwargs): check_takeoff=client.active_client.TAKEOFF_CHECK, check_land=client.active_client.LAND_CHECK, ) - print("Start action: {}".format(start_action)) + logger.debug("Start action: {}".format(start_action)) # Save corrected animation animation.save_corrected_animation(corrected_frames) return result @@ -281,7 +327,7 @@ def _response_animation_id(*args, **kwargs): @messaging.request_callback("batt_voltage") def _response_batt(*args, **kwargs): if check_state_topic(wait_new_status=True): - return FlightLib.get_telemetry('body').voltage + return FlightLib.get_telemetry_locked('body').voltage else: stop_subscriber() return float('nan') @@ -290,7 +336,7 @@ def _response_batt(*args, **kwargs): @messaging.request_callback("cell_voltage") def _response_cell(*args, **kwargs): if check_state_topic(wait_new_status=True): - return FlightLib.get_telemetry('body').cell_voltage + return FlightLib.get_telemetry_locked('body').cell_voltage else: stop_subscriber() return float('nan') @@ -301,11 +347,15 @@ def _response_sys_status(*args, **kwargs): @messaging.request_callback("cal_status") def _response_cal_status(*args, **kwargs): - return get_calibration_status() + if check_state_topic(wait_new_status=True): + return get_calibration_status() + else: + stop_subscriber() + return "NOT_CONNECTED_TO_FCU" @messaging.request_callback("position") def _response_position(*args, **kwargs): - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format( telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.FRAME_ID) @@ -323,32 +373,29 @@ def _calibrate_level(*args, **kwargs): @messaging.message_callback("test") def _command_test(*args, **kwargs): logger.info("logging info test") + rospy.logdebug("ros logdebug test") print("stdout test") @messaging.message_callback("move_start") def _command_move_start_to_current_position(*args, **kwargs): - # 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_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, - z_ratio=client.active_client.Z_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, - ) - x_start = corrected_frames[0]['x'] - y_start = corrected_frames[0]['y'] - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) - 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() - print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) + 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) + 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)) + else: + logger.debug ("Wrong telemetry") + else: + logger.debug("Wrong animation file") @messaging.message_callback("reset_start") def _command_reset_start(*args, **kwargs): @@ -356,22 +403,22 @@ def _command_reset_start(*args, **kwargs): client.active_client.config.set('PRIVATE', 'y0', 0) client.active_client.rewrite_config() client.active_client.load_config() - print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) + logger.info ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) @messaging.message_callback("set_z_to_ground") def _command_set_z(*args, **kwargs): - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + 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() - print ("Set z offset to {:.2f}".format(client.active_client.Z0)) + logger.info ("Set z offset to {:.2f}".format(client.active_client.Z0)) @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() - print ("Reset z offset to {:.2f}".format(client.active_client.Z0)) + logger.info ("Reset z offset to {:.2f}".format(client.active_client.Z0)) @messaging.message_callback("update_repo") @@ -396,7 +443,9 @@ def _command_reboot(*args, **kwargs): @messaging.message_callback("service_restart") def _command_service_restart(*args, **kwargs): - restart_service(kwargs["name"]) + service = kwargs["name"] + restart_service(service) + @messaging.message_callback("repair_chrony") def _command_chrony_repair(*args, **kwargs): @@ -426,7 +475,7 @@ def _copter_flip(*args, **kwargs): @messaging.message_callback("takeoff") def _command_takeoff(*args, **kwargs): - print("Takeoff at {}".format(datetime.datetime.now())) + logger.info("Takeoff at {}".format(datetime.datetime.now())) task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, @@ -440,8 +489,8 @@ 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(client.active_client.FRAME_ID) - print("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) + telem = FlightLib.get_telemetry_locked(client.active_client.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, @@ -497,12 +546,12 @@ def _play_animation(*args, **kwargs): start_time = float(kwargs["time"]) # Check if animation file is available if animation.get_id() == 'No animation': - print("Can't start animation without animation file!") + logger.error("Can't start animation without animation file!") return task_manager.reset(interrupt_next_task=False) - print("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time())) + 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, @@ -545,7 +594,6 @@ def _play_animation(*args, **kwargs): frame_time = rfp_time + client.active_client.RFP_TIME elif start_action == 'arm': - print ("Start_time") # Calculate start time start_time += start_delay # Arm @@ -568,6 +616,7 @@ def _play_animation(*args, **kwargs): ) # Calculate first frame start time frame_time += client.active_client.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) @@ -592,16 +641,90 @@ def _play_animation(*args, **kwargs): "use_leds": client.active_client.USE_LEDS, }, ) - #print(task_manager.task_queue) - +def telemetry_loop(): + global telemetry + rate = rospy.Rate(client.active_client.TELEM_FREQ) + while not rospy.is_shutdown(): + telemetry = telemetry._replace(animation_id = animation.get_id()) + telemetry = telemetry._replace(git_version = subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True)) + 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 + if not math.isnan(x_start): + telemetry = telemetry._replace(start_position = '{:.2f} {:.2f} {:.2f}'.format(x_start+x_delta, y_start+y_delta, z_delta)) + else: + telemetry = telemetry._replace(start_position = 'NO_POS') + services_unavailable = FlightLib.check_ros_services_unavailable() + if not services_unavailable: + try: + ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) + if ros_telemetry.connected: + telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage)) + batt_empty_param = get_param('BAT_V_EMPTY') + batt_charged_param = get_param('BAT_V_CHARGED') + batt_cells_param = get_param('BAT_N_CELLS') + if batt_empty_param.success and batt_charged_param.success and batt_cells_param.success: + batt_empty = batt_empty_param.value.real + batt_charged = batt_charged_param.value.real + batt_cells = batt_cells_param.value.integer + try: + telemetry = telemetry._replace(battery_p = '{}'.format(int(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100)))) + except ValueError: + telemetry = telemetry._replace(battery_p = 'nan') + else: + telemetry = telemetry._replace(battery_p = 'nan') + telemetry = telemetry._replace(calibration_status = get_calibration_status()) + telemetry = telemetry._replace(system_status = get_sys_status()) + telemetry = telemetry._replace(mode = ros_telemetry.mode) + check = FlightLib.selfcheck() + if not check: + check = "OK" + telemetry = telemetry._replace(selfcheck = str(check)) + if not math.isnan(ros_telemetry.x): + telemetry = telemetry._replace(current_position = '{:.2f} {:.2f} {:.2f} {:.1f} {}'.format(ros_telemetry.x, ros_telemetry.y, ros_telemetry.z, + math.degrees(ros_telemetry.yaw), client.active_client.FRAME_ID)) + else: + telemetry = telemetry._replace(current_position = 'NO_POS in {}'.format(client.active_client.FRAME_ID)) + else: + telemetry = telemetry._replace(battery_v = 'nan') + telemetry = telemetry._replace(battery_p = 'nan') + telemetry = telemetry._replace(calibration_status = 'NO_FCU') + telemetry = telemetry._replace(system_status = 'NO_FCU') + telemetry = telemetry._replace(mode = 'NO_FCU') + telemetry = telemetry._replace(selfcheck = 'NO_FCU') + telemetry = telemetry._replace(current_position = 'NO_POS') + except rospy.ServiceException: + logger.debug("Some service is unavailable") + except AttributeError as e: + logger.debug(e) + except rospy.TransportException as e: + logger.debug(e) + else: + telemetry = telemetry._replace(selfcheck = 'WAIT_ROS') + if client.active_client.TELEM_TRANSMIT: + try: + client.active_client.server_connection.send_message('telem', args={'message':create_telemetry_message(telemetry)}) + except AttributeError as e: + logger.debug(e) + + rate.sleep() + +def create_telemetry_message(telemetry): + msg = client.active_client.client_id + '`' + for key in telemetry.__dict__: + msg += telemetry.__dict__[key] + '`' + msg += repr(time.time()) + return msg + +telemetry_thread = threading.Thread(target=telemetry_loop, name="Telemetry getting thread") if __name__ == "__main__": + copter_client = CopterClient() task_manager = tasking.TaskManager() 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") diff --git a/Drone/mavros_mavlink.py b/Drone/mavros_mavlink.py index 7f58180..feded7c 100644 --- a/Drone/mavros_mavlink.py +++ b/Drone/mavros_mavlink.py @@ -78,20 +78,17 @@ def get_calibration_status(): mag_status = get_param('CAL_MAG0_ID') acc_status = get_param('CAL_ACC0_ID') status_text = "" - if gyro_status.success == False: - status_text += "gyro: wrong_param; " - elif gyro_status.value.integer == 0: + if gyro_status.value.integer == 0 and gyro_status.success: status_text += "gyro: uncalibrated; " - if mag_status.success == False: - status_text += "mag: wrong_param; " - elif mag_status.value.integer == 0: + if mag_status.value.integer == 0 and mag_status.success: status_text += "mag: uncalibrated; " - if acc_status.success == False: - status_text += "acc: wrong_param; " - elif acc_status.value.integer == 0: + if acc_status.value.integer == 0 and acc_status.success: status_text += "acc: uncalibrated; " if status_text == "": - status_text = "OK" + if not gyro_status.success or not mag_status.success or not acc_status.success: + status_text = "NO_INFO" + else: + status_text = "OK" else: status_text = status_text[:-2] return status_text diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 76a5234..6f7edca 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -87,7 +87,7 @@ class TaskManager(object): def start(self): #print("Task manager is started") - #logger.info("Task manager is started") + logger.info("Task manager is started") self._processor_thread.start() self.resume() @@ -107,7 +107,7 @@ class TaskManager(object): self._wait_interrupt_event.set() self._task_interrupt_event.set() self._running_event.clear() - #logger.info("Task queue paused") + logger.info("Task queue paused") #print("Task queue paused") def resume(self, time_to_start_next_task=0.0): @@ -118,7 +118,7 @@ class TaskManager(object): self._wait_interrupt_event.clear() self._task_interrupt_event.clear() self._running_event.set() - #logger.info("Task queue resumed with timeshift {}".format(self._timeshift)) + logger.info("Task queue resumed with timeshift {}".format(self._timeshift)) #print("Task queue resumed with timeshift {}".format(self._timeshift)) def reset(self, interrupt_next_task=True): @@ -128,11 +128,13 @@ class TaskManager(object): self._reset_event.set() def execute_task(self): + delta = 0.1 + with self._task_queue_lock: try: start_time, priority, count, task = self.task_queue[0] except Exception as e: - #print("Task queue checking exception: {}".format(e)) + logger.debug("Task queue checking exception: {}".format(e)) self._timeshift = 0.0 self._wait_interrupt_event.clear() self._task_interrupt_event.clear() @@ -140,24 +142,32 @@ class TaskManager(object): return task_start_time = start_time + self._timeshift - #logger.info("Waiting util task execution time:{}".format(task_start_time)) + logger.info("Executing task {}".format(task.func.__name__)) + logger.debug("Waiting util task execution time:{}".format(task_start_time)) #print("Waiting until task execution time:{}".format(task_start_time)) wait(task_start_time, self._wait_interrupt_event) + if task_start_time - time.time() > 0.01: + logger.error("Need for waiting more") + self._wait_interrupt_event.clear() + return + if not self._wait_interrupt_event.is_set(): #logger.info("Executing task {}".format(task)) #print("{} Executing task {}".format(time.time(),task)) #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)) if str(e) == 'STOP': self.reset() + logger.error("Return after STOP exception, can't arm!") return else: - #logger.warning("Task interrupted before execution") + logger.error("Task interrupted before execution") #print("Task interrupted before execution") self._wait_interrupt_event.clear() return @@ -166,14 +176,17 @@ class TaskManager(object): try: start_time_n, priority_n, count_n, task_n = self.task_queue[0] except Exception as e: - #print("Timeout checking exception: {}".format(e)) + logger.warning("Timeout checking exception: {}".format(e)) self._timeshift = 0.0 self._wait_interrupt_event.clear() self._task_interrupt_event.clear() self._running_event.clear() return if (task_n == task) and (start_time_n == start_time): - self.pop_task() + try: + self.pop_task() + except KeyError as e: + logger.error(str(e)) #try: #print("Pop {} function!".format(task.func.__name__)) #except Exception as e: @@ -182,11 +195,11 @@ class TaskManager(object): if self._task_interrupt_event.is_set(): self._task_interrupt_event.clear() - #logger.info("Execution done") + logger.info("Execution done") #print("Execution done") def _task_processor(self): - #logger.info("Tasking thread started") + logger.info("Tasking thread started") # self._update_queue() # Initial tick if tasks added before start while not self._shutdown_event.is_set(): self._running_event.wait() diff --git a/README.md b/README.md index cbde450..7ec58c2 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ # clever-show + [Русская версия](README_RU.md) Software for making the drone show controlled by Raspberry Pi and COEX [Clever](https://github.com/CopterExpress/clever) package. @@ -6,16 +7,16 @@ Software for making the drone show controlled by Raspberry Pi and COEX [Clever]( [![Build Status](https://travis-ci.org/CopterExpress/clever-show.svg?branch=master)](https://travis-ci.org/CopterExpress/clever-show) ## This software includes + * [Drone side](https://github.com/CopterExpress/clever-show/tree/master/Drone) with autonomous flight module, animation player module and client application for remote synchronized control of drones * [Server side](https://github.com/CopterExpress/clever-show/tree/master/Server) for making the drone show with ability of tuning drones, animation and music * [Blender 2.8 addon](https://github.com/CopterExpress/clever-show/tree/master/blender-addon) for animation export to drone paths * [Raspberry Pi image](https://github.com/CopterExpress/clever-show/releases/latest) for quick launch software on the drones ## Documentation + > Documentation is available only in Russian for now. Start tutorial is located [here](docs/ru/start-tutorial.md). Detailed documentation is located in the [docs](https://github.com/CopterExpress/clever-show/tree/master/docs) folder. - - diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 0905635..245e4a8 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -1,5 +1,6 @@ import sys import re +import math import collections import indexed from server import ConfigOption @@ -13,11 +14,10 @@ ModelStateRole = 999 class CopterData: - class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('anim_id', None), - ('batt_v', None), ('batt_p', None), - ('sys_status', None), ('cal_status', None), ('selfcheck', None), - ('position', None), ("time_delta", None), - ("client", None), ]) + class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('git_ver', None), ('anim_id', None), + ('battery', None), ('sys_status', None), ('cal_status', None), + ('mode', None), ('selfcheck', None), ('position', None), + ('start_pos', None), ('time_delta', None), ('client', None)]) def __init__(self, **kwargs): self.attrs_dict = self.class_basic_attrs.copy() @@ -63,8 +63,18 @@ class StatedCopterData(CopterData): class Checks: all_checks = {} - takeoff_checklist = (2, 3, 4, 5, 6) + takeoff_checklist = (3, 4, 6, 7, 8) + current_position = 'NO_POS' + start_position = 'NO_POS' + @classmethod + def get_pos_delta(self): + if self.current_position != 'NO_POS' and self.start_position != 'NO_POS': + delta_squared = 0 + for i in range(3): + delta_squared += (self.current_position[i]-self.start_position[i])**2 + return math.sqrt(delta_squared) + return 'NO_POS' class CopterDataModel(QtCore.QAbstractTableModel): selected_ready_signal = QtCore.pyqtSignal(bool) @@ -75,8 +85,8 @@ class CopterDataModel(QtCore.QAbstractTableModel): def __init__(self, parent=None): super(CopterDataModel, self).__init__(parent) - self.headers = ('copter ID', ' animation ID ', 'batt V', 'batt %', ' system ', - 'calibration', 'selfcheck', 'current x y z yaw frame_id', 'time delta') + 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.on_id_changed = None @@ -86,7 +96,6 @@ class CopterDataModel(QtCore.QAbstractTableModel): def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()): rows = len(contents) position = len(self.data_contents) if position == 'last' else position - self.beginInsertRows(parent, position, position + rows - 1) self.data_contents[position:position] = contents @@ -263,25 +272,25 @@ def col_check(col): @col_check(1) +def check_ver(item): + if not item: + return None + return True + +@col_check(2) def check_anim(item): if not item: return None return str(item) != 'No animation' - -@col_check(2) -def check_bat_v(item): - if not item: - return None - return float(item) > 3.2 - - @col_check(3) -def check_bat_p(item): +def check_bat(item): if not item: return None - return float(item) > 30 - + if item == "NO_INFO": + return False + else: + return float(item.split(' ')[1][:-1]) > 50 @col_check(4) def check_sys_status(item): @@ -289,29 +298,57 @@ def check_sys_status(item): return None return item == "STANDBY" - @col_check(5) def check_cal_status(item): if not item: return None return item == "OK" - @col_check(6) +def check_mode(item): + if not item: + return None + return (item != "NO_FCU") and not ("CMODE" in item) + + +@col_check(7) def check_selfcheck(item): if not item: return None return item == "OK" -@col_check(7) -def check_cal_status(item): +@col_check(8) +def check_pos_status(item): if not item: return None - return True + str_pos = item.split(' ') + if str_pos[0] != 'nan' and str_pos[0] != 'NO_POS': + Checks.current_position = [] + for i in range(3): + Checks.current_position.append(float(str_pos[i])) + return True + Checks.current_position = 'NO_POS' + return False + +@col_check(9) +def check_start_pos_status(item): + if not item: + return None + str_start_pos = item.split(' ') + if str_start_pos[0] != 'nan' and str_start_pos[0] != 'NO_POS': + Checks.start_position = [] + for i in range(3): + Checks.start_position.append(float(str_start_pos[i])) + delta = Checks.get_pos_delta() + if delta == 'NO_POS': + return False + else: + return delta < 1. + return False -@col_check(8) +@col_check(10) def check_time_delta(item): if not item: return None @@ -334,7 +371,7 @@ def takeoff_checks(copter_item): def flip_checks(copter_item): for col in Checks.takeoff_checklist: - if col != 4: + if col != 4 or col != 7: if not Checks.all_checks[col](copter_item[col]): return False else: diff --git a/Server/server_config.ini b/Server/server_config.ini index 5160e2d..8fdfddc 100644 --- a/Server/server_config.ini +++ b/Server/server_config.ini @@ -1,7 +1,7 @@ [SERVER] port = 25000 buffer_size = 1024 -remove_disconnected = True +remove_disconnected = False [BROADCAST] use_broadcast = True @@ -11,4 +11,4 @@ broadcast_delay = 5 [NTP] use_ntp = False host = ntp1.stratum2.ru -port = 123 \ No newline at end of file +port = 123 diff --git a/Server/server_gui.py b/Server/server_gui.py index dcc652c..09f2a17 100644 --- a/Server/server_gui.py +++ b/Server/server_gui.py @@ -13,7 +13,7 @@ from PyQt5 import QtCore, QtGui, QtWidgets class Ui_MainWindow(object): def setupUi(self, MainWindow): MainWindow.setObjectName("MainWindow") - MainWindow.resize(1220, 750) + MainWindow.resize(1360, 761) self.centralwidget = QtWidgets.QWidget(MainWindow) self.centralwidget.setEnabled(True) self.centralwidget.setObjectName("centralwidget") @@ -35,9 +35,10 @@ class Ui_MainWindow(object): self.tableView.setWordWrap(True) self.tableView.setObjectName("tableView") self.tableView.horizontalHeader().setCascadingSectionResizes(False) - self.tableView.horizontalHeader().setDefaultSectionSize(125) + self.tableView.horizontalHeader().setDefaultSectionSize(50) self.tableView.horizontalHeader().setMinimumSectionSize(50) self.tableView.horizontalHeader().setStretchLastSection(True) + self.tableView.verticalHeader().setVisible(False) self.horizontalLayout.addWidget(self.tableView) self.verticalLayout = QtWidgets.QVBoxLayout() self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) @@ -186,7 +187,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, 25)) + self.menubar.setGeometry(QtCore.QRect(0, 0, 1360, 25)) self.menubar.setObjectName("menubar") self.menuOptions = QtWidgets.QMenu(self.menubar) self.menuOptions.setObjectName("menuOptions") @@ -269,6 +270,7 @@ class Ui_MainWindow(object): self.menuDrone.addAction(self.action_remove_row) self.menuDrone.addSeparator() self.menuDrone.addAction(self.menuDeveloper_mode_2.menuAction()) + self.menuDrone.addAction(self.action_remove_row) self.menuMusic.addAction(self.action_select_music_file) self.menuMusic.addAction(self.action_play_music) self.menuMusic.addAction(self.action_stop_music) @@ -316,7 +318,7 @@ class Ui_MainWindow(object): self.action_send_Aruco_map.setText(_translate("MainWindow", "Send aruco map")) self.action_update_client_repo.setText(_translate("MainWindow", "Update clever-show git")) self.actionSend_launch_file_for_clever.setText(_translate("MainWindow", "Send launch file for clever")) - self.action_send_launch_file.setText(_translate("MainWindow", "Send launch file to clever")) + self.action_send_launch_file.setText(_translate("MainWindow", "Send launch files")) self.action_restart_clever.setText(_translate("MainWindow", "Restart clever service")) self.action_restart_clever_show.setText(_translate("MainWindow", "Restart clever-show service")) self.action_select_all_rows.setText(_translate("MainWindow", "Select all drones")) diff --git a/Server/server_gui.ui b/Server/server_gui.ui index 36569a1..e1b5831 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -6,8 +6,8 @@ 0 0 - 1220 - 750 + 1360 + 761 @@ -50,7 +50,7 @@ false - 125 + 50 50 @@ -58,6 +58,9 @@ true + + false + @@ -375,7 +378,7 @@ 0 0 - 1220 + 1360 25 @@ -425,6 +428,7 @@ + @@ -466,7 +470,7 @@ - Send launch file to clever + Send launch files diff --git a/Server/server_qt.py b/Server/server_qt.py index d690d82..2de6411 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -16,6 +16,7 @@ from quamash import QEventLoop, QThreadExecutor from server_gui import Ui_MainWindow from server import * +import messaging_lib as messaging from copter_table_models import * from emergency import * @@ -54,7 +55,6 @@ def confirmation_required(text="Are you sure?", label="Confirm operation?"): return inner - # noinspection PyArgumentList,PyCallByClass class MainWindow(QtWidgets.QMainWindow): def __init__(self): @@ -113,14 +113,17 @@ class MainWindow(QtWidgets.QMainWindow): self.signals.add_client_signal.emit(StatedCopterData(copter_id=client.copter_id, client=client)) def client_connection_changed(self, client: Client): + logging.debug("Start remove {}".format(client.copter_id)) row_data = self.model.get_row_by_attr("client", client) row_num = self.model.get_row_index(row_data) + logging.debug("Removing {}".format(client.copter_id)) if row_num is not None: if Server().remove_disconnected and (not client.connected): client.remove() self.signals.remove_client_signal.emit(row_num) else: self.signals.update_data_signal.emit(row_num, 0, client.connected, ModelStateRole) + logging.debug("{} removed".format(client.copter_id)) def init_ui(self): # Connecting @@ -170,59 +173,40 @@ class MainWindow(QtWidgets.QMainWindow): def selfcheck_selected(self): for copter_data_row in self.model.user_selected(): client = copter_data_row.client + client.get_response("telemetry", self.update_table_data) - client.get_response("anim_id", self.set_copter_data, callback_args=(1, copter_data_row)) - client.get_response("batt_voltage", self.set_copter_data, callback_args=(2, copter_data_row)) - client.get_response("cell_voltage", self.set_copter_data, callback_args=(3, copter_data_row)) - client.get_response("sys_status", self.set_copter_data, callback_args=(4, copter_data_row)) - client.get_response("cal_status", self.set_copter_data, callback_args=(5, copter_data_row)) - client.get_response("selfcheck", self.set_copter_data, callback_args=(6, copter_data_row)) - client.get_response("position", self.set_copter_data, callback_args=(7, copter_data_row)) - client.get_response("time", self.set_copter_data, callback_args=(8, copter_data_row)) - - def set_copter_data(self, value, col, copter_data_row): - row = self.model.get_row_index(copter_data_row) - if row is None: - logging.error("No such client!") - return - - if col == 1: - data = value - elif col == 2: - data = "{}".format(round(float(value), 3)) - elif col == 3: - batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100 # TODO config - data = "{}".format(round(batt_percent, 3)) - elif col == 4: - data = str(value) - elif col == 5: - data = str(value) - elif col == 6: - data = value - elif col == 7: - data = str(value) - elif col == 8: - data = "{}".format(round(float(value) - time.time(), 3)) - if abs(float(data)) > 1: - copter_data_row.client.send_message("repair_chrony") + @pyqtSlot(str) + def update_table_data(self, message): + fields = message.split('`') + # copter_id git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position copter_time + copter_id = fields[0] + git_version = fields[1] + animation_id = fields[2] + battery_v = fields[3] + battery_p = fields[4] + if battery_v == 'nan' or battery_p == 'nan': + battery_info = "NO_INFO" else: - logging.error("No column matched for response") - return - - self.signals.update_data_signal.emit(row, col, data, ModelDataRole) - - - #def set_copter_id(self, value, copter_data_row): - # col = 0 - # row = self.model.get_row_index(copter_data_row) - # if row is None: - # logging.error("No such client!") - # return - # logging.info("SET COPTER ID TO {}".format(value)) - # - # copter_data_row.client.copter_id = value - # self.signals.update_data_signal.emit(row, col, value, ModelDataRole) - # self.signals.update_data_signal.emit(row, col, True, ModelStateRole) + battery_info = "{}V {}%".format(battery_v, battery_p) + sys_status = fields[5] + cal_status = fields[6] + mode = fields[7] + selfcheck = fields[8] + current_pos = fields[9] + start_pos = fields[10] + copter_time = fields[11] + time_delta = "{}".format(round(float(copter_time) - time.time(), 3)) + row = self.model.get_row_index(self.model.get_row_by_attr('copter_id', copter_id)) + self.signals.update_data_signal.emit(row, 1, git_version, ModelDataRole) + self.signals.update_data_signal.emit(row, 2, animation_id, ModelDataRole) + self.signals.update_data_signal.emit(row, 3, battery_info, ModelDataRole) + self.signals.update_data_signal.emit(row, 4, sys_status, ModelDataRole) + self.signals.update_data_signal.emit(row, 5, cal_status, ModelDataRole) + self.signals.update_data_signal.emit(row, 6, mode, ModelDataRole) + self.signals.update_data_signal.emit(row, 7, selfcheck, ModelDataRole) + self.signals.update_data_signal.emit(row, 8, current_pos, ModelDataRole) + self.signals.update_data_signal.emit(row, 9, start_pos, ModelDataRole) + self.signals.update_data_signal.emit(row, 10, time_delta, ModelDataRole) @pyqtSlot(QtCore.QModelIndex) def selfcheck_info_dialog(self, index): @@ -248,10 +232,16 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def remove_selected(self): for copter in self.model.user_selected(): - row_num = self.model.data_contents.index(copter) - copter.client.remove() - self.signals.remove_client_signal.emit(row_num) - logging.info("Client removed from table!") + row_num = self.model.get_row_index(copter) + if row_num is not None: + copter.client.remove() + + if not Server().remove_disconnected: + self.signals.remove_client_signal.emit(row_num) + + logging.info("Client removed from table!") + else: + logging.error("Client is not in table!") @pyqtSlot() @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?") @@ -263,7 +253,7 @@ class MainWindow(QtWidgets.QMainWindow): music_dt = self.ui.music_delay_spin.value() asyncio.ensure_future(self.play_music_at_time(music_dt+time_now), loop=loop) logging.info('Wait {} seconds to play music'.format(music_dt)) - self.selfcheck_selected() + # self.selfcheck_selected() for copter in self.model.user_selected(): if all_checks(copter): server.send_starttime(copter.client, dt+time_now) @@ -366,13 +356,12 @@ class MainWindow(QtWidgets.QMainWindow): print("Selected directory:", path) files = [file for file in glob.glob(path + '/*.csv')] names = [os.path.basename(file).split(".")[0] for file in files] - # print(files) for file, name in zip(files, names): for copter in self.model.user_selected(): if name == copter.copter_id: copter.client.send_file(file, "animation.csv") # TODO config else: - print("Filename has no matches with any drone selected") + logging.info("Filename has no matches with any drone selected") @pyqtSlot() def send_calibrations(self): @@ -388,7 +377,7 @@ class MainWindow(QtWidgets.QMainWindow): if name == copter.copter_id: copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/camera_info/calibration.yaml") else: - print("Filename has no matches with any drone selected") + logging.info("Filename has no matches with any drone selected") @pyqtSlot() def send_configurations(self): @@ -401,7 +390,7 @@ class MainWindow(QtWidgets.QMainWindow): for section in sendable_config.sections(): for option in dict(sendable_config.items(section)): value = sendable_config[section][option] - logging.debug("Got item from config:".format(section, option, value)) + logging.debug("Got item from config: {} {} {}".format(section, option, value)) options.append(ConfigOption(section, option, value)) for copter in self.model.user_selected(): @@ -419,12 +408,14 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def send_launch(self): - path = QFileDialog.getOpenFileName(self, "Select launch file for clever", filter="Launch files (*.launch)")[0] + path = str(QFileDialog.getExistingDirectory(self, "Select directory with launch files")) if path: - filename = os.path.basename(path) - print("Selected file:", path, filename) + print("Selected directory:", path) + files = [file for file in glob.glob(path + '/*.launch')] for copter in self.model.user_selected(): - copter.client.send_file(path, "/home/pi/catkin_ws/src/clever/clever/launch/{}".format(filename)) + for file in files: + filename = os.path.basename(file) + copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/launch/{}".format(filename)) # copter.client.send_message("service_restart", {"name": "clever"}) @pyqtSlot() @@ -563,6 +554,11 @@ class MainWindow(QtWidgets.QMainWindow): self.model.data_contents[row_num].client \ .send_message("disarm") +@messaging.message_callback("telem") +def get_telem_data(*args, **kwargs): + message = kwargs.get("message", None) + window.update_table_data(message) + if __name__ == "__main__": app = QtWidgets.QApplication(sys.argv) diff --git a/builder/assets/clever-show.service b/builder/assets/clever-show.service index 33d65a1..e6b96aa 100644 --- a/builder/assets/clever-show.service +++ b/builder/assets/clever-show.service @@ -1,6 +1,8 @@ [Unit] Description=Clever Show Client Service -After=clever.service +Requires=clever.service +Requires=network.target +After=network.target [Service] WorkingDirectory=/home/pi/clever-show/Drone diff --git a/messaging_lib.py b/messaging_lib.py index 357e7f1..9b1074b 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -5,6 +5,7 @@ import json import socket import struct import random +import inspect import logging import threading import collections @@ -20,6 +21,7 @@ except ImportError: PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", "callback", "callback_args", "callback_kwargs", + "request_args", "resend", ]) logger = logging.getLogger(__name__) @@ -33,7 +35,7 @@ def get_ip_address(): ip_socket.connect(("8.8.8.8", 80)) return ip_socket.getsockname()[0] except OSError: - logging.warning("No network connection detected, using localhost") + logger.warning("No network connection detected, using localhost") return "localhost" @@ -193,7 +195,7 @@ class ConnectionManager(object): messages_callbacks = {} requests_callbacks = {} - def __init__(self): + def __init__(self, whoami = "computer"): self.selector = None self.socket = None self.addr = None @@ -203,6 +205,8 @@ class ConnectionManager(object): self._recv_buffer = b"" self._send_buffer = b"" + self.whoami = whoami + self._send_queue = collections.deque() self._received_queue = collections.deque() self._request_queue = collections.OrderedDict() @@ -213,6 +217,7 @@ class ConnectionManager(object): self.BUFFER_SIZE = 1024 self.resume_queue = False + self.resend_requests = True def _set_selector_events_mask(self, mode): """Set selector to listen for events: mode is 'r', 'w', 'rw'.""" @@ -226,7 +231,7 @@ class ConnectionManager(object): raise ValueError("Invalid events mask mode {}.".format(mode)) key = self.selector.modify(self.socket, events, data=self) - logging.debug("Switched selector of {} to mode {}".format(self.addr, key.events)) + logger.debug("Switched selector of {} to mode {}".format(self.addr, key.events)) return key def connect(self, client_selector, client_socket, client_addr): @@ -235,6 +240,8 @@ class ConnectionManager(object): self.addr = client_addr self._set_selector_events_mask('r') + if self.resend_requests: + self._resend_requests() def close(self): with self._close_lock: @@ -245,6 +252,12 @@ class ConnectionManager(object): def _close(self): logger.info("Closing connection to {}".format(self.addr)) + + if not self.resume_queue: + self._recv_buffer = b'' + self._send_buffer = b'' + self._received_queue.clear() # + try: logger.info("Unregistering selector of {}".format(self.addr)) self.selector.unregister(self.socket) @@ -298,7 +311,7 @@ class ConnectionManager(object): self._received_queue[0].income_raw = b'' if self._received_queue: - if self._received_queue[-1].content: + if self._received_queue[0].content: self.process_received(self._received_queue.popleft()) def _read(self): @@ -313,8 +326,6 @@ class ConnectionManager(object): logger.debug("Received {} from {}".format(data, self.addr)) else: logger.warning("Connection to {} lost!".format(self.addr)) - if not self.resume_queue: - self._recv_buffer = b'' raise RuntimeError("Peer closed.") @@ -357,21 +368,20 @@ class ConnectionManager(object): def _process_response(self, message): request_id, requested_value = message.content["request_id"], message.content["requested_value"] + with self._request_lock: - for key, value in self._request_queue.items(): # TODO as try [] - if (key == request_id) and (value.requested_value == requested_value): - request = self._request_queue.pop(key) - value = message.content["value"] - logger.debug( - "Request {} successfully closed with value {}".format(request, message.content["value"]) - ) + request = self._request_queue.pop(request_id, None) - f = request.callback - f(value, *request.callback_args, **request.callback_kwargs) + if (request is not None) and (request.requested_value == requested_value): + value = message.content["value"] + logger.debug( + "Request {} successfully closed with value {}".format(request, message.content["value"]) + ) - break - else: - logger.warning("Unexpected response!") + f = request.callback + f(value, *request.callback_args, **request.callback_kwargs) + else: + logger.warning("Unexpected response!") def _process_filetransfer(self, message): # TODO path? if message.jsonheader["content-type"] == "binary": @@ -383,7 +393,9 @@ class ConnectionManager(object): logger.error("File {} can not be written due error: {}".format(filepath, error)) else: logger.info("File {} successfully received ".format(filepath)) - os.system("chown -R pi:pi /home/pi/clever-show") + if self.whoami == "pi": + logger.info("Return rights to pi:pi after file transfer") + os.system("chown pi:pi {}".format(filepath)) def write(self): with self._send_lock: @@ -418,7 +430,7 @@ class ConnectionManager(object): self._send_queue.append(data) if self.selector.get_key(self.socket).events != selectors.EVENT_WRITE: - self._set_selector_events_mask('w') + self._set_selector_events_mask('rw') NotifierSock().notify() def get_response(self, requested_value, callback, request_args=None, # timeout=30, @@ -437,9 +449,22 @@ class ConnectionManager(object): callback=callback, callback_args=callback_args, callback_kwargs=callback_kwargs, + request_args=request_args, + resend=True, ) self._send(MessageManager.create_request(requested_value, request_id, request_args)) + def _resend_requests(self): + with self._request_lock: + for request_id, request in self._request_queue.items(): + if request.resend: + self._send(MessageManager.create_request( + request.requested_value, request_id, request.request_args.update(resend=request.resend)) + ) + #request.resend = False + + # self._request_queue.clear() + def send_message(self, command, args=None): self._send(MessageManager.create_simple_message(command, args)) @@ -459,7 +484,7 @@ class ConnectionManager(object): )) -class NotifierSock(Singleton): +class NotifierSock(Singleton): #TODO remake as connecting ONLY to self socket and selector def __init__(self): self.receive_socket = None self.addr = None diff --git a/tools/cut.py b/tools/cut.py new file mode 100644 index 0000000..c81025d --- /dev/null +++ b/tools/cut.py @@ -0,0 +1,100 @@ +import argparse +import os +import csv +import glob +import copy +import logging + +def cut_file(filename, _from, _to): + imported_frames = [] + anim_id = "" + + try: + animation_file = open(filename) + except IOError: + logging.error("File {} can't be opened".format(filepath)) + else: + with animation_file: + csv_reader = csv.reader( + animation_file, delimiter=',', quotechar='|' + ) + row_0 = csv_reader.next() + if len(row_0) == 1: + anim_id = row_0[0] + logging.debug("Got animation_id: {}".format(anim_id)) + else: + logging.debug("No animation id in file") + frame_number, x, y, z, yaw, red, green, blue = row_0 + 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), + }) + 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), + }) + + if _to == 0 or _to >= len(imported_frames): + _to = len(imported_frames)-1 + + path = '{}/cut_{}_{}'.format(os.path.dirname(filename),_from,_to) + print('Path is {}'.format(path)) + + csv_file = open(path+'/'+os.path.basename(filename), mode='w+') + with csv_file: + csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) + if anim_id != "": + csv_writer.writerow([anim_id]) + for i in range(_from, _to+1): + csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'], + imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']]) + + print("Successfully created file {}".format(path+'/'+filename)) + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="cut animation") + parser.add_argument('directory', nargs='?', default='.', + help="Directory with animation csv files. Default is '.'") + parser.add_argument('-f','--frm', type=int, default=0, + help="Cut from this frame, default is 0 (from the beginning)") + parser.add_argument('-t','--to', type=int, default=0, + help="Cut to this frame (including this one), default is 0 (to the end)") + args = parser.parse_args() + + _from = args.frm + _to = args.to + + path = '{}/cut_{}_{}'.format(args.directory,_from,_to) + + try: + os.mkdir(path) + except OSError: + print("Creation of the directory %s failed" % path) + files = [f for f in glob.glob(args.directory + '/*.csv')] + for f in files: + cut_file(f, _from, _to) + else: + print("Successfully created the directory %s " % path) + + files = [f for f in glob.glob(args.directory + '/*.csv')] + for f in files: + cut_file(f, _from, _to) + + + +