diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 40d019e..1ce4fe3 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") @@ -76,7 +77,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,15 +89,22 @@ 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 = 0.1 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") diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index c9913c7..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,17 +35,17 @@ 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: anim_id = "Empty id" - print("No animation id in file") + 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: - logging.error("File {} can't be opened".format(filepath)) + logger.error("File {} can't be opened".format(filepath)) anim_id = "No animation" return float('nan'), float('nan') else: @@ -59,14 +59,14 @@ def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1): return float('nan'), float('nan') 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)) try: frame_number, x, y, z, yaw, red, green, blue = csv_reader.next() except: return float('nan'), float('nan') else: anim_id = "Empty id" - print("No animation id in file") + logger.debug("No animation id in file") try: frame_number, x, y, z, yaw, red, green, blue = row_0 except: @@ -90,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/copter_client.py b/Drone/copter_client.py index 666fa36..94284c0 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -1,4 +1,5 @@ import os +import sys import time import math import rospy @@ -6,6 +7,7 @@ import datetime import logging import threading import subprocess +import ConfigParser from collections import namedtuple from FlightLib import FlightLib @@ -27,20 +29,46 @@ 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(), -# ]) +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') @@ -65,7 +93,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): @@ -73,38 +112,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() 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)) @@ -117,7 +149,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(" ") @@ -125,11 +157,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: @@ -139,7 +171,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 @@ -151,7 +183,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) @@ -162,7 +194,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 @@ -174,7 +206,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", ) @@ -187,7 +219,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 @@ -202,7 +234,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 @@ -215,7 +247,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 @@ -270,7 +302,7 @@ 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, @@ -284,7 +316,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 @@ -338,6 +370,7 @@ 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") @@ -346,20 +379,20 @@ def _command_move_start_to_current_position(*args, **kwargs): x_ratio=client.active_client.X_RATIO, y_ratio=client.active_client.Y_RATIO, ) - print("x_start = {}, y_start = {}".format(x_start, y_start)) + logger.debug("x_start = {}, y_start = {}".format(x_start, y_start)) if not math.isnan(x_start): telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) - print("x_telem = {}, y_telem = {}".format(telem.x, telem.y)) - if not math.isnan(x_telem): + 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() - print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) + logger.info ("Set start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) else: - print ("Wrong telemetry") + logger.debug ("Wrong telemetry") else: - print("Wrong animation file") + logger.debug("Wrong animation file") @messaging.message_callback("reset_start") def _command_reset_start(*args, **kwargs): @@ -367,7 +400,7 @@ 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): @@ -375,14 +408,14 @@ def _command_set_z(*args, **kwargs): 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") @@ -407,7 +440,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): @@ -437,7 +472,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, @@ -452,7 +487,7 @@ 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())) + 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, @@ -508,12 +543,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, @@ -556,7 +591,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 @@ -579,6 +613,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) @@ -603,7 +638,6 @@ def _play_animation(*args, **kwargs): "use_leds": client.active_client.USE_LEDS, }, ) - #print(task_manager.task_queue) def telemetry_loop(): global telemetry @@ -611,60 +645,67 @@ def telemetry_loop(): 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)) - try: - ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID) - except: - pass + 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: - if ros_telemetry.connected == False: - 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 in {}'.format(client.active_client.FRAME_ID)) - telemetry = telemetry._replace(start_position = 'NO_POS') - stop_subscriber() - start_subscriber() - else: - 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') - if batt_empty_param.success and batt_charged_param.success: - batt_empty = batt_empty_param.value.real - batt_charged = batt_charged_param.value.real - telemetry = telemetry._replace(battery_p = '{:.2f}'.format((ros_telemetry.cell_voltage - batt_empty)/(batt_charged - batt_empty)*100.)) + telemetry = telemetry._replace(start_position = 'NO_POS') + services_unavailable = FlightLib.check_ros_services_unavailable() + if not services_unavailable: + try: + ros_telemetry = FlightLib.get_telemetry(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 + telemetry = telemetry._replace(battery_p = '{}'.format(int(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100)))) + 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 = 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)) - 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') + 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: + rospy.logdebug("Some service is unavailable") + except AttributeError as e: + rospy.logdebug(e) + except rospy.TransportException as e: + rospy.logdebug(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 Exception as e: - print e + except AttributeError as e: + rospy.logdebug(e) + rate.sleep() def create_telemetry_message(telemetry): @@ -677,11 +718,7 @@ def create_telemetry_message(telemetry): 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/Server/copter_table_models.py b/Server/copter_table_models.py index 43440a0..dd669bf 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -13,11 +13,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,7 +62,7 @@ class StatedCopterData(CopterData): class Checks: all_checks = {} - takeoff_checklist = (2, 3, 4, 5, 6) + takeoff_checklist = (3, 4, 6, 7, 8) class CopterDataModel(QtCore.QAbstractTableModel): @@ -75,8 +74,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 ', 'calibration', + ' mode ', 'selfcheck', 'current x y z yaw frame_id', ' start x y z ', 'dt') self.data_contents = [] self.on_id_changed = None @@ -86,7 +85,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 +261,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]) > 30 @col_check(4) def check_sys_status(item): @@ -289,30 +287,40 @@ 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) +@col_check(8) def check_pos_status(item): if not item: return None - return str(item).split(' ')[0] != 'nan' + return item.split(' ')[0] != 'nan' and item.split(' ')[0] != 'NO_POS' + +@col_check(9) +def check_start_pos_status(item): + if not item: + return None + return str(item).split(' ')[0] != 'NO_POS' - -@col_check(8) +@col_check(10) def check_time_delta(item): if not item: return None @@ -335,7 +343,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_gui.py b/Server/server_gui.py index 23b1306..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, 761) + 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, 26)) + self.menubar.setGeometry(QtCore.QRect(0, 0, 1360, 25)) self.menubar.setObjectName("menubar") self.menuOptions = QtWidgets.QMenu(self.menubar) self.menuOptions.setObjectName("menuOptions") diff --git a/Server/server_gui.ui b/Server/server_gui.ui index 31a0090..e1b5831 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -6,7 +6,7 @@ 0 0 - 1220 + 1360 761 @@ -50,7 +50,7 @@ false - 125 + 50 50 @@ -58,6 +58,9 @@ true + + false + @@ -375,8 +378,8 @@ 0 0 - 1220 - 26 + 1360 + 25 diff --git a/Server/server_qt.py b/Server/server_qt.py index 3462ec0..298bce8 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -169,51 +169,6 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.takeoff_button.setEnabled(False) self.ui.flip_button.setEnabled(False) - @pyqtSlot() - def selfcheck_selected_old(self): - for copter_data_row in self.model.user_selected(): - client = copter_data_row.client - - 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") - else: - logging.error("No column matched for response") - return - - self.signals.update_data_signal.emit(row, col, data, ModelDataRole) - @pyqtSlot() def selfcheck_selected(self): for copter_data_row in self.model.user_selected(): @@ -223,13 +178,16 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot(str) def update_table_data(self, message): fields = message.split('`') - logging.info(fields[8]) # 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: + battery_info = "{}V {}%".format(battery_v, battery_p) sys_status = fields[5] cal_status = fields[6] mode = fields[7] @@ -237,28 +195,18 @@ class MainWindow(QtWidgets.QMainWindow): 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)) - logging.info("Row = {}".format(row)) - self.signals.update_data_signal.emit(row, 1, animation_id, ModelDataRole) - self.signals.update_data_signal.emit(row, 2, battery_v, ModelDataRole) - self.signals.update_data_signal.emit(row, 3, battery_p, ModelDataRole) + 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, selfcheck, ModelDataRole) - self.signals.update_data_signal.emit(row, 7, current_pos, ModelDataRole) - self.signals.update_data_signal.emit(row, 8, "{}".format(round(float(copter_time) - time.time(), 3)), 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) + 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): @@ -408,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): @@ -430,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): @@ -443,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(): diff --git a/messaging_lib.py b/messaging_lib.py index d8d02fc..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 @@ -34,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" @@ -194,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 @@ -204,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() @@ -228,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): @@ -390,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 pi:pi {}".format(filepath)) + 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: