From 61576540c0dbf72a1af92ae6aeee927dd564ccad Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sun, 8 Sep 2019 15:29:51 +0300 Subject: [PATCH] copter_client: Small fixes in animation playing logic --- Drone/animation_lib.py | 7 ++--- Drone/copter_client.py | 64 ++++++++++++++++++++---------------------- 2 files changed, 33 insertions(+), 38 deletions(-) diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 958b002..b4f16b5 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -89,16 +89,15 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): def correct_animation(frames, frame_delay=0.1, min_takeoff_height=0.5, move_delta=0.01, check_takeoff=True, check_land=True): corrected_frames = copy.deepcopy(frames) start_action = 'takeoff' - start_delay = 0. + frames_to_start = 0 if len(corrected_frames) == 0: raise Exception('Nothing to correct!') # Check takeoff - # If copter takes off in animation, set_position can be used + # If copter takes off in animation file, copter must be armed first and then all animation can be played if (corrected_frames[0]['z'] < min_takeoff_height) and check_takeoff: - start_action = 'set_position' + start_action = 'arm' # If the first point is low, then detect moment to arm, # delete all points, where copter is standing, and count time_delta - frames_to_start = 0 for i in range(len(corrected_frames)-1): if corrected_frames[i-frames_to_start+1]['z'] - corrected_frames[i-frames_to_start]['z'] > move_delta: break diff --git a/Drone/copter_client.py b/Drone/copter_client.py index e4cd288..88d5fce 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -204,26 +204,26 @@ def _command_resume(**kwargs): @messaging.message_callback("start") def _play_animation(**kwargs): - start_time = float(kwargs["time"]) # TODO - land_time = 0. - + start_time = float(kwargs["time"]) + # Check if animation file is available if animation.get_id() == 'No animation': print("Can't start animation without animation file!") return print("Start time = {}, wait for {} seconds".format(start_time, time.time() - start_time)) - + # Load animation frames = animation.load_animation(os.path.abspath("animation.csv"), x0=client.active_client.X0 + client.active_client.X0_COMMON, y0=client.active_client.Y0 + client.active_client.Y0_COMMON, ) - + # Correct start and land frames in animation corrected_frames, start_action, start_delay = animation.correct_animation(frames, check_takeoff=client.active_client.TAKEOFF_CHECK, check_land=client.active_client.LAND_CHECK, ) - + # Choose start action if start_action == 'takeoff': + # Takeoff first task_manager.add_task(start_time, 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, @@ -233,7 +233,7 @@ def _play_animation(**kwargs): "use_leds": client.active_client.USE_LEDS, } ) - + # Fly to first point rfp_time = start_time + client.active_client.TAKEOFF_TIME task_manager.add_task(rfp_time, 0, animation.execute_frame, task_kwargs={ @@ -244,42 +244,38 @@ def _play_animation(**kwargs): "flight_func": FlightLib.reach_point, } ) - + # Calculate first frame start time frame_time = rfp_time + client.active_client.RFP_TIME - for frame in corrected_frames: - point, color, yaw = animation.convert_frame(frame) - task_manager.add_task(frame_time, 0, animation.execute_frame, - task_kwargs={ - "point": point, - "color": color, - "frame_id": client.active_client.FRAME_ID, - "use_leds": client.active_client.USE_LEDS, - "flight_func": FlightLib.navto, - } - ) - frame_time += client.active_client.FRAME_DELAY - land_time = frame_time + client.active_client.LAND_TIME - else: + elif start_action == 'arm' + # Calculate start time start_time += start_delay + # Arm task_manager.add_task(start_time, 0, FlightLib.arming_wrapper, task_kwargs={ "state": True } ) + # Calculate first frame start time frame_time = start_time + 0.5 # TODO Think about arming time - for frame in corrected_frames: - point, color, yaw = animation.convert_frame(frame) # TODO add param to calculate delta - task_manager.add_task(frame_time, 0, animation.execute_frame, - task_kwargs={ - "point": point, - "color": color, - "frame_id": client.active_client.FRAME_ID, - "use_leds": client.active_client.USE_LEDS, - "flight_func": FlightLib.navto, - } - ) - land_time = frame_time + + # Play animation file + for frame in corrected_frames: + point, color, yaw = animation.convert_frame(frame) + task_manager.add_task(frame_time, 0, animation.execute_frame, + task_kwargs={ + "point": point, + "color": color, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + "flight_func": FlightLib.navto, + } + ) + frame_time += client.active_client.FRAME_DELAY + + # Calculate land_time + land_time = frame_time + client.active_client.LAND_TIME + # Land task_manager.add_task(land_time, 0, animation.land, task_kwargs={ "timeout": client.active_client.TAKEOFF_TIME,