diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 6d2c4ad..b75e745 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -6,7 +6,6 @@ import numpy import rospy import logging import threading -import ConfigParser try: from FlightLib import FlightLib @@ -21,13 +20,66 @@ logger = logging.getLogger(__name__) interrupt_event = threading.Event() -def moving(f1, f2, delta, x = True, y = True, z = True): - return ((abs(f1['x'] - f2['x']) > delta) and x - or (abs(f1['y'] - f2['y']) > delta) and y - or (abs(f1['z'] - f2['z']) > delta) and z) +def moving(f1, f2, delta, x=True, y=True, z=True): + return ((abs(f1.x - f2.x) > delta) and x + or (abs(f1.y - f2.y) > delta) and y + or (abs(f1.z - f2.z) > delta) and z) + +def get_numbers(frames): + numbers = [] + if frames: + for frame in frames: + numbers.append(frame.number) + return numbers + +class Frame(object): + params_dict = { + "number": None, + "x": None, + "y": None, + "z": None, + "yaw": None, + "red": None, + "green": None, + "blue": None, + "delay": None, + } + def __init__(self, csv_row=None, delay=None): + for key, value in self.params_dict.items(): + setattr(self, key, value) + if csv_row: + self.load_csv_row(csv_row) + if delay: + self.delay = delay + + def load_csv_row(self, csv_row): + number, x, y, z, yaw, red, green, blue = csv_row + self.number = int(number) + self.x = float(x) + self.y = float(y) + self.z = float(z) + self.yaw = float(yaw) + self.red = int(red) + self.green = int(green) + self.blue = int(blue) + + def get_pos(self): + if None in [self.x, self.y, self.z]: + return [] + else: + return [self.x, self.y, self.z] + + def get_color(self): + if None in [self.red, self.green, self.blue]: + return [] + else: + return [self.red, self.green, self.blue] + + def pose_is_valid(self): + return self.get_pos() and (self.yaw is not None) class Animation(object): - def __init__(self, config = None, filepath = "animation.csv"): + def __init__(self, config=None, filepath="animation.csv"): self.id = None self.static_begin_time = None self.takeoff_time = None @@ -43,7 +95,7 @@ class Animation(object): if config is not None: self.update_frames(config, filepath) - def load(self, delay=0.1, filepath="animation.csv"): + def load(self, filepath="animation.csv", delay=0.1): self.original_frames = [] self.corrected_frames = [] self.filepath = filepath @@ -67,42 +119,32 @@ class Animation(object): logger.debug("Got new frame delay: {}".format(current_frame_delay)) else: logger.debug("No animation id in file") - frame_number, x, y, z, yaw, red, green, blue = row_0 - self.original_frames.append({ - 'number': int(frame_number), - 'x': float(x), - 'y': float(y), - 'z': float(z), - 'yaw': float(yaw), - 'red': int(red), - 'green': int(green), - 'blue': int(blue), - 'delay': current_frame_delay - }) + try: + frame = Frame(row_0, current_frame_delay) + except ValueError as e: + logger.error("Can't parse row in csv file. {}".format(e)) + return + else: + self.original_frames.append(frame) for row in csv_reader: if len(row) == 2: current_frame_delay = float(row[1]) logger.debug("Got new frame delay: {}".format(current_frame_delay)) else: - frame_number, x, y, z, yaw, red, green, blue = row - self.original_frames.append({ - 'number': int(frame_number), - 'x': float(x), - 'y': float(y), - 'z': float(z), - 'yaw': float(yaw), - 'red': int(red), - 'green': int(green), - 'blue': int(blue), - 'delay': current_frame_delay - }) + try: + frame = Frame(row, current_frame_delay) + except ValueError as e: + logger.error("Can't parse row in csv file. {}".format(e)) + return + else: + self.original_frames.append(frame) self.split_animation() ''' Split animation into 5 parts: static_begin, takeoff, route, land, static_end - * static_begin and static_end are arrays of frames in the beginning and the end of animation, + * static_begin and static_end are chains of frames in the beginning and the end of animation, where the drone doesn't move - * takeoff and land are arrays of frames after and before static frames of animation, + * takeoff and land are chains of frames after and before static frames of animation, where the drone doesn't move in xy plane, and it's z coordinate only increases or decreases, respectively. * route is the rest of the animation Count static_begin_time and takeoff_time @@ -123,36 +165,41 @@ class Animation(object): while i < len(frames) - 1: if moving(frames[i], frames[i+1], move_delta): break - self.static_begin_time += frames[i]['delay'] + self.static_begin_time += frames[i].delay i += 1 - self.static_begin_frames = frames[:i+1] - frames = frames[i+1:] - i = 0 + if i > 0: + self.static_begin_frames = frames[:i+1] + frames = frames[i+1:] + i = 0 # Select takeoff frames while i < len(frames) - 1: - if moving(frames[i], frames[i+1], move_delta, z = False) or frames[i]['z'] - frames[i+1]['z'] <= 0: + if moving(frames[i], frames[i+1], move_delta, z = False) or (frames[i+1].z - frames[i].z <= 0): break - self.takeoff_time += frames[i]['delay'] + self.takeoff_time += frames[i].delay i += 1 - self.takeoff_frames = frames[:i+1] - frames = frames[i+1:] + if i > 0: + self.takeoff_frames = frames[:i+1] + frames = frames[i+1:] i = len(frames) - 1 # Moving index from the end # Select static end frames while i >= 0: if moving(frames[i], frames[i-1], move_delta): break i -= 1 - self.static_end_frames = frames[i:] - frames = frames[:i] - i -= len(frames) - 1 + if i < len(frames) - 1: + self.static_end_frames = frames[i+1:] + frames = frames[:i+1] + i = len(frames) - 1 # Select land frames while i >= 0: - if moving(frames[i], frames[i-1], move_delta, z = False) or frames[i-1]['z'] - frames[i]['z'] >= 0: + if moving(frames[i], frames[i-1], move_delta, z = False) or (frames[i-1].z - frames[i].z <= 0): break i -= 1 - self.land_frames = frames[i:] + if i < len(frames) - 1: + self.land_frames = frames[i+1:] + frames = frames[:i+1] # Get route frames - self.route_frames = frames[:i] + self.route_frames = frames def make_output_frames(self, static_begin, takeoff, route, land, static_end): self.output_frames = [] @@ -166,24 +213,25 @@ class Animation(object): self.output_frames += self.land_frames if static_end: self.output_frames += self.static_end_frames - self.output_frames_min_z = min(self.output_frames, key = lambda p: p['z'])['z'] + self.output_frames_min_z = min(self.output_frames, key = lambda p: p.z).z def update_frames(self, config, filepath): - self.load(config.animation_frame_delay, filepath) - self.make_output_frames(config.animation_output_static_begin, - config.animation_output_takeoff, - config.animation_output_route, - config.animation_output_land, - config.animation_output_static_end) + self.load(filepath, config.animation_frame_delay) + if self.original_frames: + self.make_output_frames(config.animation_output_static_begin, + config.animation_output_takeoff, + config.animation_output_route, + config.animation_output_land, + config.animation_output_static_end) def get_scaled_output(self, ratio = (1,1,1), offset = (0,0,0)): x0, y0, z0 = offset x_ratio, y_ratio, z_ratio = ratio scaled_frames = copy.deepcopy(self.output_frames) for frame in scaled_frames: - frame['x'] = x_ratio*frame['x'] + x0 - frame['y'] = y_ratio*frame['y'] + y0 - frame['z'] = z_ratio*frame['z'] + z0 + frame.x = x_ratio*frame.x + x0 + frame.y = y_ratio*frame.y + y0 + frame.z = z_ratio*frame.z + z0 return scaled_frames def get_scaled_output_min_z(self, ratio = (1,1,1), offset = (0,0,0)): @@ -195,9 +243,9 @@ class Animation(object): x0, y0, z0 = offset x_ratio, y_ratio, z_ratio = ratio first_frame = self.output_frames[0] - x = x_ratio*first_frame['x'] + x0 - y = y_ratio*first_frame['y'] + y0 - z = z_ratio*first_frame['z'] + z0 + x = x_ratio*first_frame.x + x0 + y = y_ratio*first_frame.y + y0 + z = z_ratio*first_frame.z + z0 return x, y, z def get_start_action(self, start_action, current_height, takeoff_level): @@ -221,20 +269,19 @@ class Animation(object): with open(filepath, mode='w+') as corrected_animation: csv_writer = csv.writer(corrected_animation, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) for frame in self.corrected_frames: - csv_writer.writerow([frame['number'], frame['x'], frame['y'], frame['z'], frame['red'], frame['green'], frame['blue'], frame['delay']]) - -def convert_frame(frame): - return ((frame['x'], frame['y'], frame['z']), (frame['red'], frame['green'], frame['blue']), frame['yaw']) + csv_writer.writerow([frame.number, frame.x, frame.y, frame.z, frame.red, frame.green, frame.blue, frame.delay]) try: - def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True, + def execute_frame(frame, frame_id='aruco_map', use_leds=True, flight_func=FlightLib.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event): if flight_kwargs is None: flight_kwargs = {} - - flight_func(*point, yaw=yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs) + if frame.pose_is_valid(): + flight_func(x=frame.x, y=frame.y, z=frame.z, yaw=frame.yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs) + else: + logger.debug("Frame pose is not valid for flying") if use_leds: - if color: + if frame.get_color: LedLib.fill(*color) def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True, diff --git a/Drone/config/spec/configspec_client.ini b/Drone/config/spec/configspec_client.ini index 4678684..6e8ac74 100644 --- a/Drone/config/spec/configspec_client.ini +++ b/Drone/config/spec/configspec_client.ini @@ -71,6 +71,12 @@ frame_delay = float(default=0.1, min=0.01) ratio = float_list(default=list(1.0, 1.0, 1.0), min=3, max=3) # Available options: 'animation', 'nan' 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) [LED] use = boolean(default=False)