From 14e416c4f977e67491d945ff67c958208e9f1a73 Mon Sep 17 00:00:00 2001 From: artem30801 Date: Fri, 12 May 2023 12:48:20 +0100 Subject: [PATCH] fix: animation reading errors --- drone/client.py | 89 +++++++++++++++++++---------------- drone/modules/animation.py | 15 ++++-- drone/tests/animation_test.py | 1 - 3 files changed, 59 insertions(+), 46 deletions(-) diff --git a/drone/client.py b/drone/client.py index cdde932..0a8fe62 100644 --- a/drone/client.py +++ b/drone/client.py @@ -176,7 +176,8 @@ class CopterClient(client_core.Client): def on_config_update(self): self.gps_thread_run = False self.load_config() - self.animation.on_config_update(self.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": @@ -422,7 +423,8 @@ def _response_telemetry(*args, **kwargs): @messaging.request_callback("anim_id") def _response_animation_id(*args, **kwargs): # Load animation - return copter.animation.id + with copter.animation.lock: + return copter.animation.id @messaging.request_callback("batt_voltage") @@ -501,7 +503,8 @@ 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: - xs, ys, zs = copter.animation.get_start_frame(copter.telemetry.start_action).get_pos() + 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: @@ -688,33 +691,34 @@ def _play_animation(*args, **kwargs): logger.error("start: No time argument!") return - # Check animation state - if copter.animation.state is not "OK": - logger.error("start: Bad animation state") - 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 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 + # 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) + # 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: @@ -772,18 +776,21 @@ class Telemetry: return "{} V{}".format(copter.config.config_name, copter.config.config_version) def get_start_position(self): - try: - x, y, z = copter.animation.get_start_frame('fly').get_pos() - except (ValueError, AttributeError): - 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' + 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: - 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] + 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): @@ -850,7 +857,8 @@ class Telemetry: self.round_telemetry() def update_telemetry_slow(self): - self.animation_info = [copter.animation.id, copter.animation.state] + 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() @@ -998,7 +1006,8 @@ class AnimationEventHandler(FileSystemEventHandler): logger.info("Config updated!") elif (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted"): if os.path.exists("animation.csv"): - copter.animation.on_animation_update("animation.csv") + with copter.animation.lock: + copter.animation.on_animation_update("animation.csv") logger.info("Animation updated!") diff --git a/drone/modules/animation.py b/drone/modules/animation.py index 87aa315..63c1b95 100644 --- a/drone/modules/animation.py +++ b/drone/modules/animation.py @@ -1,4 +1,4 @@ -import os +import sys import csv import copy import math @@ -134,9 +134,11 @@ class Frame(object): class Animation(object): # filepath - path to csv animation file, config - config 'ANIMATION' section (dictionary) def __init__(self, filepath="animation.csv", config=None): - self.reset(filepath, config) - if config is not None: - self.on_animation_update(self.filepath) + self.lock = threading.Lock() + with self.lock: + self.reset(filepath, config) + if config is not None: + self.on_animation_update(self.filepath) def reset(self, filepath, config): self.id = None @@ -179,7 +181,10 @@ class Animation(object): animation_file, delimiter=',', quotechar='|' ) try: - row_0 = csv_reader.next() + if sys.version_info[0] == 3: + row_0 = next(csv_reader) + else: + row_0 = csv_reader.next() except StopIteration: self.set_state("Animation file is empty", log_error=True) return diff --git a/drone/tests/animation_test.py b/drone/tests/animation_test.py index 534509f..a6f06a7 100644 --- a/drone/tests/animation_test.py +++ b/drone/tests/animation_test.py @@ -7,7 +7,6 @@ import logging 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),