diff --git a/Drone/config/spec/configspec_client.ini b/Drone/config/spec/configspec_client.ini index 6e8ac74..1574baa 100644 --- a/Drone/config/spec/configspec_client.ini +++ b/Drone/config/spec/configspec_client.ini @@ -39,11 +39,11 @@ decrease_thrust_after = float(default=5.0, min=0) [COPTER] frame_id = string(default=map) +arming_time = float(default=0.5) takeoff_height = float(default=1.0) takeoff_time = float(default=5.0, min=0) -safe_takeoff = boolean(default=False) reach_first_point_time = float(default=5.0, min=0) -land_time = float(default=1.0, min=0) +land_delay = float(default=1.0, min=0) land_timeout = float(default=10.0, min=0) # __list__ x y z common_offset = float_list(default=list(0, 0, 0), min=3, max=3) @@ -63,20 +63,28 @@ lon = string(default=0) yaw = float(default=0) [ANIMATION] -takeoff_detection = boolean(default=True) -land_detection = boolean(default=True) +# Available options: +# 'auto' - automatic action selection from 'takeoff' or 'fly' based on current copter level +# 'takeoff' - takeoff to first output animation point after static_begin_time then execute 'takeoff logic' +# 'fly' - execute animation frames after static_begin_time +start_action = string(default=auto) +takeoff_level = float(default=0.5) +ground_level = float(default=0) frame_delay = float(default=0.1, min=0.01) # Animation ratio (x, y, z) # __list__ x y z ratio = float_list(default=list(1.0, 1.0, 1.0), min=3, max=3) -# Available options: 'animation', 'nan' or a number in degrees +# Available options: +# 'animation' - take yaw from animation +# 'nan' - don't change yaw during flight +# or a number in degrees yaw = string(default=180.0) - [[OUTPUT]] - static_begin = boolean(default=False) - takeoff = boolean(default=True) - route = boolean(default=True) - land = boolean(default=False) - static_end = boolean(default=False) +[[OUTPUT]] +static_begin = boolean(default=False) +takeoff = boolean(default=True) +route = boolean(default=True) +land = boolean(default=False) +static_end = boolean(default=False) [LED] use = boolean(default=False) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 83534d4..8b87daf 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -9,7 +9,10 @@ import numpy try: from clever import srv except ImportError: - from clover import srv + try: + from clover import srv + except ImportError: + print("Can't import clever or clover") import datetime import logging @@ -17,8 +20,17 @@ import threading import psutil import subprocess from collections import namedtuple +from watchdog.observers import Observer +from watchdog.events import FileSystemEventHandler -from FlightLib import FlightLib +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 @@ -46,6 +58,12 @@ def azi(x, y): 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 @@ -96,8 +114,8 @@ class CopterClient(client.Client): def __init__(self, config_path="config/client.ini"): super(CopterClient, self).__init__(config_path) self.load_config() - self.frames = {} self.telemetry = None + self.animation = animation.Animation(self.config, "animation.csv") def load_config(self): super(CopterClient, self).load_config() @@ -111,7 +129,7 @@ class CopterClient(client.Client): if self.config.led_use: from FlightLib import LedLib LedLib.init_led(self.config.led_pin) - task_manager_instance.start() # TODO move to self + task_manager_instance.start() start_subscriber() self.telemetry = Telemetry() self.telemetry.start_loop() @@ -158,11 +176,11 @@ class CopterClient(client.Client): 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'])) + #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)) + #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 @@ -314,13 +332,13 @@ def _execute(*args, **kwargs): def _response_id(*args, **kwargs): new_id = kwargs.get("new_id", None) if new_id is not None: - old_id = client.active_client.client_id + old_id = copter.client_id if new_id != old_id: - client.active_client.config.set('PRIVATE', 'id', new_id, write=True) - client.active_client.client_id = new_id + copter.config.set('PRIVATE', 'id', new_id, write=True) + copter.client_id = new_id if new_id != '/hostname': - if client.active_client.config.system_restart_after_rename: - hostname = client.active_client.client_id + if copter.config.system_restart_after_rename: + hostname = copter.client_id configure_hostname(hostname) configure_hosts(hostname) configure_bashrc(hostname) @@ -347,28 +365,14 @@ def _response_selfcheck(*args, **kwargs): @messaging.request_callback("telemetry") def _response_telemetry(*args, **kwargs): - telemetry.update() - return telemetry.create_msg_contents() + copter.telemetry.update() + return copter.telemetry.create_msg_contents() @messaging.request_callback("anim_id") def _response_animation_id(*args, **kwargs): # Load animation - result = animation.get_id() - if result != 'No animation': - logger.debug("Saving corrected animation") - offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset) - frames = animation.load_animation(os.path.abspath("animation.csv"), client.active_client.config.animation_frame_delay, - offset[0], offset[1], offset[2], *client.active_client.config.animation_ratio) - # Correct start and land frames in animation - corrected_frames, start_action, start_delay = animation.correct_animation(frames, - check_takeoff=client.active_client.config.animation_takeoff_detection, - check_land=client.active_client.config.animation_land_detection, - ) - logger.debug("Start action: {}".format(start_action)) - # Save corrected animation - animation.save_corrected_animation(corrected_frames) - return result + return copter.animation.id @messaging.request_callback("batt_voltage") @@ -405,9 +409,9 @@ def _response_cal_status(*args, **kwargs): @messaging.request_callback("position") def _response_position(*args, **kwargs): - telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id) + telem = copter.telemetry.ros_telemetry return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format( - telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.config.copter_frame_id) + telem.x, telem.y, telem.z, math.degrees(telem.yaw), copter.config.copter_frame_id) @messaging.request_callback("calibrate_gyro") @@ -436,60 +440,61 @@ def _command_test(*args, **kwargs): 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): - x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"), - *client.active_client.config.animation_ratio) - logger.debug("x_start = {}, y_start = {}".format(x_start, y_start)) - if not math.isnan(x_start): - telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id) - logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y)) - if not math.isnan(telem.x): - client.active_client.config.set('PRIVATE', 'offset', - [telem.x - x_start, telem.y - y_start, client.active_client.config.private_offset[2]], - write=True) - logger.info("Set start delta: {:.2f} {:.2f}".format(client.active_client.config.private_offset[0], - client.active_client.config.private_offset[1])) - else: - logger.debug("Wrong telemetry") + 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("Wrong animation file") + 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): - client.active_client.config.set('PRIVATE', 'offset', - [0, 0, client.active_client.config.private_offset[2]], - write=True) - logger.info("Reset start to {:.2f} {:.2f}".format(client.active_client.config.private_offset[0], - client.active_client.config.private_offset[1])) + 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 = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id) - client.active_client.config.set('PRIVATE', 'offset', - [client.active_client.config.private_offset[0], client.active_client.config.private_offset[1], telem.z], - write=True) - logger.info("Set z offset to {:.2f}".format(client.active_client.config.private_offset[2])) + 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): - client.active_client.config.set('PRIVATE', 'offset', - [client.active_client.config.private_offset[0], client.active_client.config.private_offset[1], 0], - write=True) - logger.info("Reset z offset to {:.2f}".format(client.active_client.config.private_offset[2])) + 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("mv /home/pi/clever-show/Drone/client_config.ini /home/pi/clever-show/Drone/client_config_tmp.ini") - os.system("git reset --hard HEAD") - os.system("git checkout master") os.system("git fetch") os.system("git pull --rebase") - os.system("mv /home/pi/clever-show/Drone/client_config_tmp.ini /home/pi/clever-show/Drone/client_config.ini") os.system("chown -R pi:pi /home/pi/clever-show") @@ -512,7 +517,7 @@ def _command_service_restart(*args, **kwargs): @messaging.message_callback("repair_chrony") def _command_chrony_repair(*args, **kwargs): - repair_chrony(client.active_client.config.server_host) + repair_chrony(copter.config.server_host) @messaging.message_callback("led_test") @@ -527,13 +532,12 @@ 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=client.active_client.config.copter_frame_id) + FlightLib.flip(frame_id=copter.config.copter_frame_id) @messaging.message_callback("takeoff") @@ -541,30 +545,36 @@ def _command_takeoff(*args, **kwargs): logger.info("Takeoff at {}".format(datetime.datetime.now())) task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ - "z": client.active_client.config.copter_takeoff_height, - "timeout": client.active_client.config.copter_takeoff_time, - "safe_takeoff": client.active_client.config.copter_safe_takeoff, - "use_leds": client.active_client.config.led_use & client.active_client.config.led_takeoff_indication, - } - ) + "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): - z_str = kwargs.get("z", None) - if z_str is not None: - telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id) - logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) - task_manager.add_task(0, 0, FlightLib.reach_point, - task_kwargs={ - "x": telem.x, - "y": telem.y, - "z": float(z_str), - "frame_id": client.active_client.config.copter_frame_id, - "timeout": client.active_client.config.copter_takeoff_time, - "auto_arm": True, - } - ) + 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") @@ -572,12 +582,11 @@ def _command_land(*args, **kwargs): task_manager.reset() task_manager.add_task(0, 0, animation.land, task_kwargs={ - "z": client.active_client.config.copter_takeoff_height, - "timeout": client.active_client.config.copter_takeoff_time, - "frame_id": client.active_client.config.copter_frame_id, - "use_leds": client.active_client.config.led_use & client.active_client.config.led_land_indication, - } - ) + "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") @@ -591,8 +600,7 @@ def _command_disarm(*args, **kwargs): task_manager.add_task(-5, 0, FlightLib.arming_wrapper, task_kwargs={ "state": False - } - ) + }) @messaging.message_callback("stop") @@ -612,109 +620,104 @@ def _command_resume(*args, **kwargs): @messaging.message_callback("start") def _play_animation(*args, **kwargs): - start_time = float(kwargs["time"]) - # Check if animation file is available - if animation.get_id() == 'No animation': - logger.error("Can't start animation without animation file!") + + # 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 + start_action, start_delay = copter.telemetry.start_action_and_delay + + # Reset task manager task_manager.reset(interrupt_next_task=False) - logger.info("Start time = {}, wait for {} seconds".format(start_time, start_time - time.time())) - # Load animation - offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset) - frames = animation.load_animation(os.path.abspath("animation.csv"), client.active_client.config.animation_frame_delay, - offset[0], offset[1], offset[2], *client.active_client.config.animation_ratio) - # Correct start and land frames in animation - corrected_frames, start_action, start_delay = animation.correct_animation(frames, - check_takeoff=client.active_client.config.animation_takeoff_detection, - check_land=client.active_client.config.animation_land_detection, - ) - # Choose start action + # Set animation logic if start_action == 'takeoff': - # Takeoff first - task_manager.add_task(start_time, 0, animation.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": client.active_client.config.copter_takeoff_height, - "timeout": client.active_client.config.copter_takeoff_time, - "safe_takeoff": client.active_client.config.copter_safe_takeoff, - # "frame_id": client.active_client.config.copter_frame_id, - "use_leds": client.active_client.config.led_use & client.active_client.config.led_takeoff_indication, - } - ) + "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 = start_time + client.active_client.config.copter_takeoff_time - if client.active_client.config.animation_yaw == "animation": - yaw = frame["yaw"] - else: - yaw = math.radians(float(client.active_client.config.animation_yaw)) + rfp_time = takeoff_time + copter.config.copter_takeoff_time task_manager.add_task(rfp_time, 0, animation.execute_frame, task_kwargs={ - "point": animation.convert_frame(corrected_frames[0])[0], - "color": animation.convert_frame(corrected_frames[0])[1], - "yaw": yaw, - "frame_id": client.active_client.config.copter_frame_id, - "use_leds": client.active_client.config.led_use, + "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 + client.active_client.config.copter_reach_first_point_time + frame_time = rfp_time + copter.config.copter_reach_first_point_time elif start_action == 'arm': # Calculate start time - start_time += start_delay - frame_time = start_time # + 1.0 - point, color, yaw = animation.convert_frame(corrected_frames[0]) - task_manager.add_task(frame_time, 0, animation.execute_frame, + arm_time = start_time + start_delay # + 1.0 + task_manager.add_task(arm_time, 0, animation.execute_frame, task_kwargs={ - "point": point, - "color": color, - "frame_id": client.active_client.config.copter_frame_id, - "use_leds": client.active_client.config.led_use, + "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 += corrected_frames[0]["delay"] # TODO Think about arming time - logger.debug(task_manager.task_queue) + frame_time = arm_time + copter.config.copter_arming_time + logger.debug("Start queue {}".format(task_manager.task_queue)) # Play animation file - for frame in corrected_frames: - point, color, yaw = animation.convert_frame(frame) - if client.active_client.config.animation_yaw == "animation": - yaw = frame["yaw"] - else: - yaw = math.radians(float(client.active_client.config.animation_yaw)) + for frame in frames: task_manager.add_task(frame_time, 0, animation.execute_frame, task_kwargs={ - "point": point, - "color": color, - "yaw": yaw, - "frame_id": client.active_client.config.copter_frame_id, - "use_leds": client.active_client.config.led_use, + "frame": frame, + "frame_id": copter.config.copter_frame_id, + "use_leds": copter.config.led_use, "flight_func": FlightLib.navto, - } - ) - frame_time += frame["delay"] - + }) + frame_time += frame.delay # Calculate land_time - land_time = frame_time + client.active_client.config.copter_land_time + land_time = frame_time + copter.config.copter_land_delay # Land task_manager.add_task(land_time, 0, animation.land, task_kwargs={ - "timeout": client.active_client.config.copter_land_timeout, - "frame_id": client.active_client.config.copter_frame_id, - "use_leds": client.active_client.config.led_use & client.active_client.config.led_land_indication, - }, - ) + "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_id": None, + "animation_info": None, "battery": None, "armed": False, "fcu_status": None, @@ -723,6 +726,7 @@ class Telemetry: "selfcheck": None, "current_position": None, "start_position": None, + "start_action_and_delay": None, "last_task": None, "time_delta": None, "config_version": None, @@ -759,16 +763,16 @@ class Telemetry: @classmethod def get_config_version(cls): - return "{} V{}".format(client.active_client.config.config_name, client.active_client.config.config_version) + return "{} V{}".format(copter.config.config_name, copter.config.config_version) @classmethod def get_start_position(cls): - x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"), - *client.active_client.config.animation_ratio) - offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset) + x_start, y_start, z_start = animation.get_start_pos(os.path.abspath("animation.csv"), + *copter.config.animation_ratio) + offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset) x = x_start + offset[0] y = y_start + offset[1] - z = offset[2] + z = z_start + offset[2] if not FlightLib._check_nans(x, y, z): return x, y, z return 'NO_POS' @@ -807,14 +811,25 @@ class Telemetry: def get_position(cls, ros_telemetry): x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z if not math.isnan(x): - return x, y, z, math.degrees(ros_telemetry.yaw), client.active_client.config.copter_frame_id + 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 get_start_action_and_delay(self) + offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset) + 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) + start_delay = copter.animation.start_delay + return start_action, start_delay + def update_telemetry_fast(self): - self.start_position = self.get_start_position() self.last_task = task_manager.get_current_task() try: - self.ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id) + 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 @@ -832,13 +847,12 @@ class Telemetry: self.time_delta = time.time() self.round_telemetry() - def get_ros_telemetry(self): - return self.ros_telemetry - def update_telemetry_slow(self): - self.animation_id = animation.get_id() + 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() + self.start_action_and_delay = self.get_start_action_and_delay() try: self.calibration_status = get_calibration_status() self.fcu_status = get_sys_status() @@ -905,7 +919,7 @@ class Telemetry: def transmit_message(self): # todo if connected try: - client.active_client.server_connection.send_message('telemetry', kwargs={'value': self.create_msg_contents()}) + copter.server_connection.send_message('telemetry', kwargs={'value': self.create_msg_contents()}) except AttributeError as e: logger.debug(e) @@ -935,7 +949,7 @@ class Telemetry: self.update_telemetry_fast() self.check_failsafe_and_interruption() - if client.active_client.config.telemetry_transmit and client.active_client.connected: + if copter.config.telemetry_transmit and copter.connected: self.transmit_message() rate.sleep() @@ -944,14 +958,14 @@ class Telemetry: rate = rospy.Rate(1) while not rospy.is_shutdown(): self.update_telemetry_slow() - if client.active_client.config.telemetry_log_resources: + if copter.config.telemetry_log_resources: self.log_cpu_and_memory() rate.sleep() def start_loop(self): - if client.active_client.config.telemetry_frequency > 0: + if copter.config.telemetry_frequency > 0: telemetry_thread = threading.Thread(target=self._update_loop, name="Telemetry getting thread", - args=(client.active_client.config.telemetry_frequency,)) # TODO MOVE? Daemon? + args=(copter.config.telemetry_frequency,)) # TODO MOVE? Daemon? slow_telemetry_thread = threading.Thread(target=self._slow_update_loop, name="Slow telemetry getting thread") slow_telemetry_thread.start() @@ -971,8 +985,23 @@ def emergency_callback(data): emergency = data.data +class AnimationEventHandler(FileSystemEventHandler): + def on_any_event(self, event): + logger.info('{} is {}'.format(event.src_path, event.event_type)) + if event.src_path == "./animation.csv" and event.event_type == "modified": + copter.animation.update_frames(copter.config, "animation.csv") + + if __name__ == "__main__": - copter_client = CopterClient() + copter = CopterClient() task_manager = tasking.TaskManager() rospy.Subscriber('/emergency', Bool, emergency_callback) - copter_client.start(task_manager) + copter.start(task_manager) + event_handler = AnimationEventHandler() + observer = Observer() + observer.schedule(event_handler, ".") + observer.start() + while not rospy.is_shutdown: + rospy.sleep(1) + observer.stop() + observer.join()