import os import sys import time import math import numpy import psutil import logging import datetime import threading import subprocess from watchdog.observers import Observer from watchdog.events import FileSystemEventHandler # Import rospy try: import rospy except ImportError: print("Can't import rospy! Please check your ROS installation.") exit() import rospkg # Import clever or clover package try: from clever import srv except ImportError: try: from clover import srv except ImportError: print("Can't import clever or clover! Please check installation of clover ROS package.") exit() # Import flight control try: import modules.flight as flight except ImportError: print("Can't import flight control module!") # Import led control try: import modules.led as led except ImportError: print("Can't import led control module!") # Add parent dir to PATH to import messaging_lib and config_lib current_dir = (os.path.dirname(os.path.realpath(__file__))) lib_dir = os.path.realpath(os.path.join(current_dir, '../lib')) sys.path.insert(0, lib_dir) import messaging import modules.client_core as client_core import modules.animation as animation import modules.mavros_wrapper as mavros import modules.tasking as tasking 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 def contains_nan(array): for num in array: if math.isnan(num): return True return False static_broadcaster = tf2_ros.StaticTransformBroadcaster() emergency = False 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(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('modules.animation') animation_logger.setLevel(logging.INFO) animation_logger.addHandler(handler) client_logger = logging.getLogger('modules.client_core') client_logger.setLevel(logging.DEBUG) client_logger.addHandler(handler) messaging_logger = logging.getLogger('messaging') messaging_logger.setLevel(logging.INFO) messaging_logger.addHandler(handler) tasking_logger = logging.getLogger('modules.tasking') tasking_logger.setLevel(logging.INFO) tasking_logger.addHandler(handler) flight_logger = logging.getLogger('modules.flight') flight_logger.setLevel(logging.INFO) flight_logger.addHandler(handler) mavros_mavlink_logger = logging.getLogger('modules.mavros_wrapper') 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() if self.config.clover_dir == 'auto': self.check_clover_dir() self.telemetry = None self.animation = animation.Animation("animation.csv", self.config) self.gps_thread_run = False self.gps_thread_is_running = False def load_config(self): super(CopterClient, self).load_config() def check_clover_dir(self): rospack = rospkg.RosPack() try: path = rospack.get_path('clever') except rospkg.common.ResourceNotFound: try: path = rospack.get_path('clover') except rospkg.common.ResourceNotFound: path = 'error' self.config.set('', 'clover_dir', path, write=True) if path.count("/pi/"): self.server_connection.whoami = "pi" 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) task_manager_instance.start() mavros.start_subscriber() self.telemetry = Telemetry() self.telemetry.start_loop() if self.config.flight_frame_id == "floor": self.start_floor_frame_broadcast() elif self.config.flight_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() def on_config_update(self): self.gps_thread_run = False self.load_config() with self.animation.lock: self.animation.on_config_update(self.config) if self.config.flight_frame_id == "floor": self.start_floor_frame_broadcast() elif self.config.flight_frame_id == "gps": self.start_gps_frame_broadcast() 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.flight_frame_id static_broadcaster.sendTransform(trans) def start_gps_frame_broadcast(self): 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): while self.gps_thread_is_running: rospy.sleep(1) logger.info("Wait until previous gps thread stop") self.gps_thread_run = True self.gps_thread_is_running = True rate = rospy.Rate(1) while not rospy.is_shutdown() and self.gps_thread_run: telem = flight.get_telemetry_locked(frame_id = "map") amsl_height = mavros.get_amsl_altitude() compass_hdg = mavros.get_compass_hdg() if telem is not None: if contains_nan([telem.lat, telem.lon, telem.x, telem.y, telem.yaw, amsl_height, compass_hdg]): logger.info("Can't get position data from GPS") 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 gps_dz = 0 if self.config.gps_frame_amsl != "current": try: gps_dz = telem.z - amsl_height + float(self.config.gps_frame_amsl) except ValueError: logger.error("Wrong amsl height in GPS FRAME config!") continue #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 = gps_dz yaw = math.radians(compass_hdg) + telem.yaw - math.radians(self.config.gps_frame_yaw) trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,yaw)) trans.header.frame_id = "map" trans.child_frame_id = "gps" static_broadcaster.sendTransform(trans) rate.sleep() self.gps_thread_is_running = False 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('', '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 mavros.check_state_topic(wait_new_status=True): check = flight.selfcheck() return check if check else "OK" else: mavros.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 with copter.animation.lock: return copter.animation.id @messaging.request_callback("batt_voltage") def _response_batt(*args, **kwargs): if mavros.check_state_topic(wait_new_status=True): return flight.get_telemetry_locked('body').voltage else: mavros.stop_subscriber() return float('nan') @messaging.request_callback("cell_voltage") def _response_cell(*args, **kwargs): if mavros.check_state_topic(wait_new_status=True): return flight.get_telemetry_locked('body').cell_voltage else: mavros.stop_subscriber() return float('nan') @messaging.request_callback("sys_status") def _response_sys_status(*args, **kwargs): return mavros.get_sys_status() @messaging.request_callback("cal_status") def _response_cal_status(*args, **kwargs): if mavros.check_state_topic(wait_new_status=True): return mavros.get_calibration_status() else: mavros.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.flight_frame_id) @messaging.request_callback("calibrate_gyro") def _calibrate_gyro(*args, **kwargs): mavros.calibrate('gyro') return mavros.get_calibration_status() @messaging.request_callback("calibrate_level") def _calibrate_level(*args, **kwargs): mavros.calibrate('level') return mavros.get_calibration_status() @messaging.request_callback("load_params") def _load_params(*args, **kwargs): result = mavros.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): pass # should be updated via watchdog event #copter.animation.update_frames(copter.config, "animation.csv") @messaging.message_callback("move_start") def _command_move_start_to_current_position(*args, **kwargs): private_offset = copter.config.animation_private_offset offset = numpy.array(private_offset) + numpy.array(copter.config.animation_common_offset) try: with copter.animation.lock: xs, ys, zs = copter.animation.get_start_frame(copter.telemetry.start_action).get_pos() 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('ANIMATION', 'private_offset', [private_offset[0] + telem.x - xs, private_offset[1] + telem.y - ys, private_offset[2]], write=True) logger.info("Set start delta: {:.2f} {:.2f}".format(copter.config.animation_private_offset[0], copter.config.animation_private_offset[1])) else: logger.error("Wrong telemetry") @messaging.message_callback("reset_start") def _command_reset_start(*args, **kwargs): copter.config.set('ANIMATION', 'private_offset', [0, 0, copter.config.animation_private_offset[2]], write=True) logger.info("Reset start to {:.2f} {:.2f}".format(copter.config.animation_private_offset[0], copter.config.animation_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('ANIMATION', 'private_offset', [copter.config.animation_private_offset[0], copter.config.animation_private_offset[1], telem.z], write=True) logger.info("Set z offset to {:.2f}".format(copter.config.animation_private_offset[2])) else: logger.error("Wrong telemetry") @messaging.message_callback("reset_z_offset") def _command_reset_z(*args, **kwargs): copter.config.set('ANIMATION', 'private_offset', [copter.config.animation_private_offset[0], copter.config.animation_private_offset[1], 0], write=True) logger.info("Reset z offset to {:.2f}".format(copter.config.animation_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): mavros.reboot_fcu() execute_command("reboot") @messaging.message_callback("reboot_fcu") def _command_reboot(*args, **kwargs): mavros.reboot_fcu() @messaging.message_callback("service_restart") def _command_service_restart(*args, **kwargs): service = kwargs["name"] if service=="clover": restart_service("clever") restart_service("clover") 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): led.set_effect(effect='flash', r=255, g=255, b=255) @messaging.message_callback("led_fill") def _command_led_fill(*args, **kwargs): red = kwargs.get("red", 0) green = kwargs.get("green", 0) blue = kwargs.get("blue", 0) led.set_effect(r=red, g=green, b=blue) @messaging.message_callback("flip") def _copter_flip(*args, **kwargs): flight.flip(frame_id=copter.config.flight_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.flight_takeoff_height, "timeout": copter.config.flight_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 = flight.get_telemetry_locked(copter.config.flight_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, flight.reach_point, task_kwargs={ "x": telem.x, "y": telem.y, "z": z, "frame_id": copter.config.flight_frame_id, "timeout": copter.config.flight_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.flight_takeoff_height, "timeout": copter.config.flight_land_timeout, "frame_id": copter.config.flight_frame_id, "use_leds": copter.config.led_use & copter.config.led_land_indication, }) @messaging.message_callback("emergency_land") def _emergency_land(*args, **kwargs): try: result = flight.emergency_land() logger.info(result.message) except rospy.ServiceException: logger.error("Can't execute emergency land: service is unavailable!") @messaging.message_callback("disarm") def _command_disarm(*args, **kwargs): task_manager.reset() task_manager.add_task(-5, 0, flight.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 with copter.animation.lock: # Check animation state if copter.animation.state is not "OK": logger.error("start: Bad animation state") return # Get output frames frames = copter.animation.get_output_frames(copter.telemetry.start_action) 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 # Play animation! frame_time = start_time for frame in frames: task_manager.add_task(frame_time, 0, animation.execute_frame, task_kwargs={ "frame": frame, "config": copter.config, }) frame_time += frame.delay task_manager.add_task(frame_time, 0, animation.turn_off_led) # 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 self.start_action = 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): result = subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True, universal_newlines=True) if isinstance(result, bytes): result = result.decode("ascii") return result.strip() @classmethod def get_config_version(cls): return "{} V{}".format(copter.config.config_name, copter.config.config_version) def get_start_position(self): with copter.animation.lock: try: logger.info(copter.animation) x, y, z = copter.animation.get_start_frame('fly').get_pos() except (ValueError, AttributeError) as e: logger.error(e) return [float('nan'),float('nan'),float('nan'),float('nan'),"error: can't get start pos in animation",float('nan')] else: start_delay = copter.animation.start_time yaw = copter.animation.get_start_frame('fly').yaw if not self.ros_telemetry: self.start_action = 'error: no telemetry data' else: self.start_action = copter.animation.get_start_action(self.ros_telemetry.z, self.fcu_status) return [x,y,z,yaw,self.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 = mavros.get_param('BAT_V_EMPTY') batt_charged_param = mavros.get_param('BAT_V_CHARGED') batt_cells_param = mavros.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 = flight.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.flight_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 = flight.get_telemetry_locked(copter.config.flight_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): with copter.animation.lock: 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 = mavros.get_calibration_status() self.fcu_status = mavros.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', 'stand']) 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() flight.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 event.src_path.split('/')[-1] == 'client.ini': if os.path.exists("animation.csv"): copter.on_config_update() logger.info("Config updated!") elif (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted"): if os.path.exists("animation.csv"): with copter.animation.lock: copter.animation.on_animation_update("animation.csv") logger.info("Animation updated!") 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)