import os import sys import time import math import rospy import numpy # for backward compatibility with clever try: from clever import srv except ImportError: try: from clover import srv except ImportError: print("Can't import clever or clover") import datetime import logging import threading import psutil import subprocess from collections import namedtuple from watchdog.observers import Observer from watchdog.events import FileSystemEventHandler try: from FlightLib import FlightLib except ImportError: print("Can't import FlightLib") try: from FlightLib import LedLib except ImportError: print("Can't import LedLib") import client_core import messaging_lib as messaging import tasking_lib as tasking import animation_lib as animation from mavros_mavlink import * from std_msgs.msg import Bool from geometry_msgs.msg import Point, Quaternion, TransformStamped from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply import tf2_ros from geographiclib.geodesic import Geodesic Earth = Geodesic.WGS84 def dist(x, y): return math.sqrt(x**2+y**2) def azi(x, y): return 90 - math.atan2(y,x)*180/math.pi def get_xy(dist, azi): return dist*math.sin(math.radians(azi)), dist*math.cos(math.radians(azi)) def valid(pos): for coord in pos: if math.isnan(coord): return False return True static_broadcaster = tf2_ros.StaticTransformBroadcaster() emergency = False 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) 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) mavros_mavlink_logger = logging.getLogger('mavros_mavlink') mavros_mavlink_logger.setLevel(logging.INFO) mavros_mavlink_logger.addHandler(handler) class CopterClient(client_core.Client): def __init__(self, config_path="config/client.ini"): super(CopterClient, self).__init__(config_path) self.load_config() self.telemetry = None self.animation = animation.Animation(self.config, "animation.csv") def load_config(self): super(CopterClient, self).load_config() def on_broadcast_bind(self): repair_chrony(self.config.server_host) def start(self, task_manager_instance): rospy.loginfo("Init ROS node") rospy.init_node('clever_show_client', anonymous=True) if self.config.led_use: from FlightLib import LedLib LedLib.init_led(self.config.led_pin) task_manager_instance.start() start_subscriber() self.telemetry = Telemetry() self.telemetry.start_loop() if self.config.copter_frame_id == "floor": self.start_floor_frame_broadcast() elif self.config.copter_frame_id == "gps": self.start_gps_frame_broadcast() client_thread = threading.Thread(target=super(CopterClient, self).start, name="Client thread") client_thread.daemon = True client_thread.start() #super(CopterClient, self).start() def start_floor_frame_broadcast(self): if self.config.floor_frame_parent == "gps": self.start_gps_frame_broadcast() trans = TransformStamped() trans.transform.translation.x = self.config.floor_frame_translation[0] trans.transform.translation.y = self.config.floor_frame_translation[1] trans.transform.translation.z = self.config.floor_frame_translation[2] trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.config.floor_frame_rotation[0]), math.radians(self.config.floor_frame_rotation[1]), math.radians(self.config.floor_frame_rotation[2]))) trans.header.frame_id = self.config.floor_frame_parent trans.child_frame_id = self.config.copter_frame_id static_broadcaster.sendTransform(trans) def start_gps_frame_broadcast(self): trans = TransformStamped() trans.transform.translation.x = 0 trans.transform.translation.y = 0 trans.transform.translation.z = 0 trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,0)) trans.header.frame_id = "map" trans.child_frame_id = "gps" static_broadcaster.sendTransform(trans) gps_frame_thread = threading.Thread(target=self.gps_frame_broadcast_loop, name="GPS frame broadcast thread") gps_frame_thread.daemon = True gps_frame_thread.start() def gps_frame_broadcast_loop(self): rate = rospy.Rate(1) while not rospy.is_shutdown(): telem = FlightLib.get_telemetry_locked(frame_id = "map") if telem is not None: if math.isnan(telem.lat) or math.isnan(telem.lon) or math.isnan(telem.x) or math.isnan(telem.y): logger.info("Can't get position from telemetry") else: lat = float(self.config.gps_frame_lat) lon = float(self.config.gps_frame_lon) geo_delta = Earth.Inverse(telem.lat, telem.lon, lat, lon) #logger.info("dist: {} | azi: {}".format(geo_delta['s12'], geo_delta['azi1'])) dx, dy = get_xy(geo_delta['s12'], geo_delta['azi1']) gps_dx = telem.x + dx gps_dy = telem.y + dy #logger.info("GPS frame dx: {} | dy: {}".format(gps_dx, gps_dy)) trans = TransformStamped() trans.transform.translation.x = gps_dx trans.transform.translation.y = gps_dy trans.transform.translation.z = 0 trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0, math.radians(self.config.gps_frame_yaw))) trans.header.frame_id = "map" trans.child_frame_id = "gps" static_broadcaster.sendTransform(trans) rate.sleep() def restart_service(name): os.system("systemctl restart {}".format(name)) def repair_chrony(ip): logger.info("Configure chrony ip to {}".format(ip)) configure_chrony_ip(ip) restart_service("chrony") def execute_command(command): os.system(command) def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): # TODO simplify try: with open(path, 'r') as f: raw_content = f.read() except IOError as e: logger.error("Reading error {}".format(e)) return False content = raw_content.split(" ") try: current_ip = content[ip_index] except IndexError: logger.error("Something wrong with config") return False if "." not in current_ip: logger.debug("That's not ip!") if current_ip != ip: content[ip_index] = ip try: with open(path, 'w') as f: f.write(" ".join(content)) except IOError: logger.error("Error writing") return False return True def configure_hostname(hostname): path = "/etc/hostname" try: with open(path, 'r') as f: raw_content = f.read() except IOError as e: logger.error("Reading error {}".format(e)) return False current_hostname = str(raw_content) if current_hostname != hostname: content = hostname + '\n' try: with open(path, 'w') as f: f.write(content) except IOError: logger.error("Error writing") return False return True def configure_hosts(hostname): path = "/etc/hosts" try: with open(path, 'r') as f: raw_content = f.read() except IOError as e: logger.error("Reading error {}".format(e)) return False index_start = raw_content.find("127.0.1.1", ) index_stop = raw_content.find("\n", index_start) hosts_array = raw_content[index_start:index_stop].split() _ip = hosts_array[0] current_hostname = hosts_array[1] if current_hostname != hostname: content = raw_content[:index_start] + "{} {} {}.local".format(_ip, hostname, hostname) + raw_content[ index_stop:] try: with open(path, 'w') as f: f.write(content) except IOError: logger.error("Error writing") return False return True def configure_motd(hostname): with open("/etc/motd", "w") as f: f.write("\r\n{}\r\n\r\n".format(hostname)) def configure_bashrc(hostname): path = "/home/pi/.bashrc" try: with open(path, 'r') as f: raw_content = f.read() except IOError as e: logger.error("Reading error {}".format(e)) return False index_start = raw_content.find("ROS_HOSTNAME='", ) + 14 index_stop = raw_content.find("'", index_start) current_hostname = raw_content[index_start:index_stop] if current_hostname != hostname: content = raw_content[:index_start] + hostname + raw_content[index_stop:] try: with open(path, 'w') as f: f.write(content) except IOError: logger.error("Error writing") return False return True @messaging.message_callback("execute") def _execute(*args, **kwargs): command = kwargs.get("command", None) if command is not None: logger.info("Executing command: {}".format(command)) execute_command(command) logger.info("Executing done") @messaging.message_callback("id") # TODO redo def _response_id(*args, **kwargs): new_id = kwargs.get("new_id", None) if new_id is not None: old_id = copter.client_id if new_id != old_id: copter.config.set('PRIVATE', 'id', new_id, write=True) copter.client_id = new_id if new_id != '/hostname': if copter.config.system_restart_after_rename: hostname = copter.client_id configure_hostname(hostname) configure_hosts(hostname) configure_bashrc(hostname) configure_motd(hostname) execute_command("systemctl stop clever-show & reboot") # execute_command("hostname {}".format(hostname)) # restart_service("dhcpcd") # restart_service("avahi-daemon") # restart_service("smbd") # restart_service("roscore") # restart_service("clever") restart_service("clever-show") @messaging.request_callback("selfcheck") def _response_selfcheck(*args, **kwargs): if check_state_topic(wait_new_status=True): check = FlightLib.selfcheck() return check if check else "OK" else: stop_subscriber() return "NOT_CONNECTED_TO_FCU" @messaging.request_callback("telemetry") def _response_telemetry(*args, **kwargs): copter.telemetry.update() return copter.telemetry.create_msg_contents() @messaging.request_callback("anim_id") def _response_animation_id(*args, **kwargs): # Load animation return copter.animation.id @messaging.request_callback("batt_voltage") def _response_batt(*args, **kwargs): if check_state_topic(wait_new_status=True): return FlightLib.get_telemetry_locked('body').voltage else: stop_subscriber() return float('nan') @messaging.request_callback("cell_voltage") def _response_cell(*args, **kwargs): if check_state_topic(wait_new_status=True): return FlightLib.get_telemetry_locked('body').cell_voltage else: stop_subscriber() return float('nan') @messaging.request_callback("sys_status") def _response_sys_status(*args, **kwargs): return get_sys_status() @messaging.request_callback("cal_status") def _response_cal_status(*args, **kwargs): 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 = copter.telemetry.ros_telemetry return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format( telem.x, telem.y, telem.z, math.degrees(telem.yaw), copter.config.copter_frame_id) @messaging.request_callback("calibrate_gyro") def _calibrate_gyro(*args, **kwargs): calibrate('gyro') return get_calibration_status() @messaging.request_callback("calibrate_level") def _calibrate_level(*args, **kwargs): calibrate('level') return get_calibration_status() @messaging.request_callback("load_params") def _load_params(*args, **kwargs): result = load_param_file('temp.params') logger.info("Load parameters to FCU success: {}".format(result)) return result @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("update_animation") def _command_update_animation(*args, **kwargs): copter.animation.update_frames(copter.config, "animation.csv") @messaging.message_callback("move_start") def _command_move_start_to_current_position(*args, **kwargs): offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset) try: xs, ys, zs = copter.animation.get_start_point(copter.config.ratio, offset) except ValueError: logger.error("Can't get start point. Check animation file!") else: logger.debug("start x = {}, y = {}".format(xs, ys)) telem = copter.telemetry.ros_telemetry logger.debug("telemetry x = {}, y = {}".format(telem.x, telem.y)) if valid([telem.x, telem.y, telem.z]): copter.config.set('PRIVATE', 'offset', [telem.x - xs, telem.y - ys, copter.config.private_offset[2]], write=True) logger.info("Set start delta: {:.2f} {:.2f}".format(copter.config.private_offset[0], copter.config.private_offset[1])) else: logger.error("Wrong telemetry") @messaging.message_callback("reset_start") def _command_reset_start(*args, **kwargs): copter.config.set('PRIVATE', 'offset', [0, 0, copter.config.private_offset[2]], write=True) logger.info("Reset start to {:.2f} {:.2f}".format(copter.config.private_offset[0], copter.config.private_offset[1])) @messaging.message_callback("set_z_to_ground") def _command_set_z(*args, **kwargs): telem = copter.telemetry.ros_telemetry if valid([telem.x, telem.y, telem.z]): copter.config.set('PRIVATE', 'offset', [copter.config.private_offset[0], copter.config.private_offset[1], telem.z], write=True) logger.info("Set z offset to {:.2f}".format(copter.config.private_offset[2])) else: logger.error("Wrong telemetry") @messaging.message_callback("reset_z_offset") def _command_reset_z(*args, **kwargs): copter.config.set('PRIVATE', 'offset', [copter.config.private_offset[0], copter.config.private_offset[1], 0], write=True) logger.info("Reset z offset to {:.2f}".format(copter.config.private_offset[2])) @messaging.message_callback("update_repo") def _command_update_repo(*args, **kwargs): os.system("git fetch") os.system("git pull --rebase") os.system("chown -R pi:pi /home/pi/clever-show") @messaging.message_callback("reboot_all") def _command_reboot_all(*args, **kwargs): reboot_fcu() execute_command("reboot") @messaging.message_callback("reboot_fcu") def _command_reboot(*args, **kwargs): reboot_fcu() @messaging.message_callback("service_restart") def _command_service_restart(*args, **kwargs): service = kwargs["name"] if service=="clover": restart_service("clever") if service=="clever-show": restart_service("clever-show@{}".format(copter.client_id)) restart_service(service) @messaging.message_callback("repair_chrony") def _command_chrony_repair(*args, **kwargs): repair_chrony(copter.config.server_host) @messaging.message_callback("led_test") def _command_led_test(*args, **kwargs): LedLib.chase(255, 255, 255) time.sleep(2) LedLib.off() @messaging.message_callback("led_fill") def _command_led_fill(*args, **kwargs): r = kwargs.get("red", 0) g = kwargs.get("green", 0) b = kwargs.get("blue", 0) LedLib.fill(r, g, b) @messaging.message_callback("flip") def _copter_flip(*args, **kwargs): FlightLib.flip(frame_id=copter.config.copter_frame_id) @messaging.message_callback("takeoff") def _command_takeoff(*args, **kwargs): logger.info("Takeoff at {}".format(datetime.datetime.now())) task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ "z": copter.config.copter_takeoff_height, "timeout": copter.config.copter_takeoff_time, "safe_takeoff": False, "use_leds": copter.config.led_use & copter.config.led_takeoff_indication, }) @messaging.message_callback("takeoff_z") def _command_takeoff_z(*args, **kwargs): try: z = float(kwargs.get("z", None)) except TypeError: logger.error("takeoff_z: No z argument!") except ValueError: logger.error("takeoff_z: Wrong z argument!") else: telem = FlightLib.get_telemetry_locked(copter.config.copter_frame_id) if valid([telem.x, telem.y, telem.z]): logger.info("Takeoff to z = {} at {}".format(z, datetime.datetime.now())) task_manager.add_task(0, 0, FlightLib.reach_point, task_kwargs={ "x": telem.x, "y": telem.y, "z": z, "frame_id": copter.config.copter_frame_id, "timeout": copter.config.copter_takeoff_time, "auto_arm": True, }) else: logger.error("Wrong telemetry!") @messaging.message_callback("land") def _command_land(*args, **kwargs): task_manager.reset() task_manager.add_task(0, 0, animation.land, task_kwargs={ "z": copter.config.copter_takeoff_height, "timeout": copter.config.copter_land_timeout, "frame_id": copter.config.copter_frame_id, "use_leds": copter.config.led_use & copter.config.led_land_indication, }) @messaging.message_callback("emergency_land") def _emergency_land(*args, **kwargs): logger.info(FlightLib.emergency_land().message) @messaging.message_callback("disarm") def _command_disarm(*args, **kwargs): task_manager.reset() task_manager.add_task(-5, 0, FlightLib.arming_wrapper, task_kwargs={ "state": False }) @messaging.message_callback("stop") def _command_stop(*args, **kwargs): task_manager.reset() @messaging.message_callback("pause") def _command_pause(*args, **kwargs): task_manager.pause() @messaging.message_callback("resume") def _command_resume(*args, **kwargs): task_manager.resume(time_to_start_next_task=kwargs.get("time", 0)) @messaging.message_callback("start") def _play_animation(*args, **kwargs): # Validate start_time try: start_time = float(kwargs["time"]) except ValueError: logger.error("start: Wrong time argument!") return except KeyError: logger.error("start: No time argument!") return # Check animation state if copter.animation.state is not "OK": logger.error("start: Bad animation state") return # Get output frames offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset) frames = copter.animation.get_scaled_output(copter.config.animation_ratio, offset) if not frames: logger.error("start: No frames in animation!") return # Get current telemetry telem = copter.telemetry.ros_telemetry if not valid([telem.x, telem.y, telem.z]): logger.error("start: Position is not valid!") return # Get start action and delay try: start_action, start_delay = copter.telemetry.start_position[-2:] except ValueError: logger.error("start: Can't get animation start position and delay") return # Reset task manager task_manager.reset(interrupt_next_task=False) # Set animation logic if start_action == 'takeoff': # Takeoff first at start_time + start_delay_time takeoff_time = start_time + start_delay task_manager.add_task(takeoff_time, 0, animation.takeoff, task_kwargs={ "z": copter.config.copter_takeoff_height, "timeout": copter.config.copter_takeoff_time, "safe_takeoff": False, "use_leds": copter.config.led_use & copter.config.led_takeoff_indication, }) # Fly to first point rfp_time = takeoff_time + copter.config.copter_takeoff_time task_manager.add_task(rfp_time, 0, animation.execute_frame, task_kwargs={ "frame": frames[0], "frame_id": copter.config.copter_frame_id, "use_leds": copter.config.led_use, "flight_func": FlightLib.reach_point, }) # Calculate first frame start time frame_time = rfp_time + copter.config.copter_reach_first_point_time elif start_action == 'fly': # Calculate start time arm_time = start_time + start_delay # + 1.0 task_manager.add_task(arm_time, 0, animation.execute_frame, task_kwargs={ "frame": frames[0], "frame_id": copter.config.copter_frame_id, "use_leds": copter.config.led_use, "flight_func": FlightLib.navto, "auto_arm": True, }) # Calculate first frame start time frame_time = arm_time + copter.config.copter_arming_time logger.debug("Start queue {}".format(task_manager.task_queue)) # Play animation file for frame in frames: task_manager.add_task(frame_time, 0, animation.execute_frame, task_kwargs={ "frame": frame, "frame_id": copter.config.copter_frame_id, "use_leds": copter.config.led_use, "flight_func": FlightLib.navto, }) frame_time += frame.delay # Calculate land_time land_time = frame_time + copter.config.copter_land_delay # Land task_manager.add_task(land_time, 0, animation.land, task_kwargs={ "timeout": copter.config.copter_land_timeout, "frame_id": copter.config.copter_frame_id, "use_leds": copter.config.led_use & copter.config.led_land_indication, }) # noinspection PyAttributeOutsideInit class Telemetry: params_default_dict = { "git_version": None, "animation_info": None, "battery": None, "armed": False, "fcu_status": None, "calibration_status": None, "mode": None, "selfcheck": None, "current_position": None, "start_position": None, "last_task": None, "time_delta": None, "config_version": None, } def __init__(self): self._lock = threading.Lock() self._last_state = [] self._interruption_counter = 0 self._max_interruptions = 2 self._tasks_cleared = False self.ros_telemetry = None for key, value in self.params_default_dict.items(): setattr(self, key, value) def __setattr__(self, key, value): if key in self.params_default_dict: with self.__dict__['_lock']: self.__dict__[key] = value else: self.__dict__[key] = value def __getattr__(self, item): if item in self.params_default_dict: with self.__dict__['_lock']: return self.__dict__[item] return self.__dict__[item] @classmethod def get_git_version(cls): return subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True) @classmethod def get_config_version(cls): return "{} V{}".format(copter.config.config_name, copter.config.config_version) def get_start_position(self): offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset) try: x, y, z = copter.animation.get_start_point(copter.config.animation_ratio, offset) except ValueError: return [float('nan'),float('nan'),float('nan'),float('nan'),'error: no start pos in animation',float('nan')] else: start_delay = copter.animation.start_delay yaw = copter.animation.get_start_yaw() if not self.ros_telemetry: start_action = 'error: no telemetry data' else: start_action = copter.animation.get_start_action( copter.config.animation_start_action, self.ros_telemetry.z, copter.config.animation_takeoff_level, copter.config.animation_ground_level, copter.config.animation_ratio, offset) return [x,y,z,yaw,start_action,start_delay] @classmethod def get_battery(cls, ros_telemetry): if ros_telemetry is None: return float('nan'), float('nan') battery_v = 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 battery_p = (ros_telemetry.voltage / batt_cells - batt_empty) / (batt_charged - batt_empty) * 1. battery_p = max(min(battery_p, 1.), 0.) else: battery_p = float('nan') return battery_v, battery_p @classmethod def get_selfcheck(cls): check = FlightLib.selfcheck() if not check: check = "OK" return check @classmethod def get_position(cls, ros_telemetry): try: x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z except AttributeError: return 'NO_POS' if not math.isnan(x): return x, y, z, math.degrees(ros_telemetry.yaw), copter.config.copter_frame_id return 'NO_POS' def get_ros_telemetry(self): return self.ros_telemetry def update_telemetry_fast(self): self.last_task = task_manager.get_current_task() try: self.ros_telemetry = FlightLib.get_telemetry_locked(copter.config.copter_frame_id) if self.ros_telemetry.connected: self.armed = self.ros_telemetry.armed self.mode = self.ros_telemetry.mode self.selfcheck = self.get_selfcheck() self.current_position = self.get_position(self.ros_telemetry) else: self.reset_telemetry_values() except rospy.ServiceException: rospy.logdebug("Some service is unavailable") self.selfcheck = ["WAIT_ROS"] except AttributeError as e: rospy.logdebug(e) except rospy.TransportException as e: rospy.logdebug(e) self.time_delta = time.time() self.round_telemetry() def update_telemetry_slow(self): self.animation_info = [copter.animation.id, copter.animation.state] self.git_version = self.get_git_version() self.config_version = self.get_config_version() self.start_position = self.get_start_position() try: self.calibration_status = get_calibration_status() self.fcu_status = get_sys_status() self.battery = self.get_battery(self.ros_telemetry) except rospy.ServiceException: rospy.logdebug("Some service is unavailable") self.selfcheck = ["WAIT_ROS"] except AttributeError as e: rospy.logdebug(e) except rospy.TransportException as e: rospy.logdebug(e) def update(self): self.update_telemetry_fast() self.update_telemetry_slow() def round_telemetry(self): round_list = ["battery", "start_position", "current_position"] for key in round_list: if self.__dict__[key] not in [None, 'NO_POS', 'NO_FCU']: self.__dict__[key] = [round(v, 2) if type(v) == float else v for v in self.__dict__[key]] def reset_telemetry_values(self): self.battery = float('nan'), float('nan') self.calibration_status = 'NO_FCU' self.fcu_status = 'NO_FCU' self.mode = 'NO_FCU' self.selfcheck = ['NO_FCU'] self.current_position = 'NO_POS' def check_failsafe_and_interruption(self): global emergency # check current state state = [self.mode, self.armed, task_manager.get_last_task_name()] mode, armed, last_task = state # check external interruption external_interruption = (mode != "OFFBOARD" and armed == True and last_task not in [None, 'land']) log_msg = '' if emergency: log_msg += 'emergency and ' if external_interruption: log_msg += 'external interruption and ' # count interruptions to avoid px4 mode glitches if state == self._last_state: self._interruption_counter += 1 else: self._interruption_counter = 0 logger.info("Possible expernal interruption, state_counter = {}".format(self._interruption_counter)) # delete last ' end ' from log message if len(log_msg) > 5: log_msg = log_msg[:-5] # clear task manager if emergency or external interruption if emergency or (external_interruption and self._interruption_counter >= self._max_interruptions): if not self._tasks_cleared: logger.info("Clear task manager because of {}".format(log_msg)) logger.info("Mode: {} | armed: {} | last task: {} ".format(mode, armed, last_task)) task_manager.reset() FlightLib.reset_delta() self._tasks_cleared = True self._interruption_counter = 0 else: self._tasks_cleared = False self._last_state = state def transmit_message(self): # todo if connected try: copter.server_connection.send_message('telemetry', kwargs={'value': self.create_msg_contents()}) except AttributeError as e: logger.debug(e) @classmethod def log_cpu_and_memory(cls): cpu_usage = psutil.cpu_percent(interval=None, percpu=True) mem_usage = psutil.virtual_memory().percent cpu_temp_info = psutil.sensors_temperatures()['cpu-thermal'][0] cpu_temp = cpu_temp_info.current # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md throttled_hex = subprocess.check_output("vcgencmd get_throttled", shell=True).split('=')[1] under_voltage = bool(int(bin(int(throttled_hex, 16))[2:][-1])) power_state = 'normal' if not under_voltage else 'under voltage!' if cpu_temp_info.critical: cpu_temp_state = 'critical' elif cpu_temp_info.high: cpu_temp_state = 'high' else: cpu_temp_state = 'normal' logger.info("CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format( cpu_usage, mem_usage, cpu_temp, cpu_temp_state, power_state)) def _update_loop(self, freq): # TODO extract? rate = rospy.Rate(freq) while not rospy.is_shutdown(): self.update_telemetry_fast() self.check_failsafe_and_interruption() if copter.config.telemetry_transmit and copter.connected: self.transmit_message() rate.sleep() def _slow_update_loop(self): rate = rospy.Rate(1) while not rospy.is_shutdown(): self.update_telemetry_slow() if copter.config.telemetry_log_resources: self.log_cpu_and_memory() rate.sleep() def start_loop(self): if copter.config.telemetry_frequency > 0: telemetry_thread = threading.Thread(target=self._update_loop, name="Telemetry getting thread", args=(copter.config.telemetry_frequency,)) telemetry_thread.daemon = True telemetry_thread.start() slow_telemetry_thread = threading.Thread(target=self._slow_update_loop, name="Slow telemetry getting thread") slow_telemetry_thread.daemon = True slow_telemetry_thread.start() else: logger.info("Telemetry loop is not created because of zero or negative telemetry frequency") def create_msg_contents(self, keys=None): # keys: set or list if keys is None: keys = self.params_default_dict.keys() # return only existing keys from 'keys' return {k: self.__dict__[k] for k in keys if k in self.params_default_dict} def emergency_callback(data): global emergency emergency = data.data class AnimationEventHandler(FileSystemEventHandler): def on_any_event(self, event): logger.info('{} is {}'.format(event.src_path, event.event_type)) # logger.info(os.path.splitext(event.src_path)) if (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted") or event.src_path.split('/')[-1] == 'client.ini': if os.path.exists("animation.csv"): logger.info("Update frames from animation.csv") copter.animation.update_frames(copter.config, "animation.csv") if __name__ == "__main__": copter = CopterClient() task_manager = tasking.TaskManager() rospy.Subscriber('/emergency', Bool, emergency_callback) event_handler = AnimationEventHandler() observer = Observer() observer.schedule(event_handler, ".", recursive=True) observer.daemon = True observer.start() copter.start(task_manager) while not rospy.is_shutdown(): rospy.sleep(0.1)