From 805f91a3292d0df1b49d3115b70bc5c4ceb6cd18 Mon Sep 17 00:00:00 2001 From: Arhur Golubtsov Date: Tue, 10 Sep 2019 11:35:27 +0100 Subject: [PATCH] Drone: Simplify takeoff function (arm is checked in navigate) --- Drone/FlightLib/FlightLib.py | 85 ++++++++++-------------------------- Drone/animation_lib.py | 7 ++- Drone/copter_client.py | 2 +- 3 files changed, 26 insertions(+), 68 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index cb653a1..d5a6c53 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -30,13 +30,13 @@ logger.info("Proxy services inited") FREQUENCY = 40 # HZ TOLERANCE = 0.2 SPEED = 1.0 -SPEED_TAKEOFF = 1.0 +TAKEOFF_SPEED = 1.0 TIMEOUT = 5.0 TIMEOUT_ARMED = 2.0 TIMEOUT_DESCEND = TIMEOUT TIMEOUT_LAND = 8.0 Z_DESCEND = 0.5 -Z_TAKEOFF = 1.0 +TAKEOFF_HEIGHT = 1.0 FRAME_ID = 'map' INTERRUPTER = threading.Event() FLIP_MIN_Z = 2.0 @@ -297,78 +297,37 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA #print("Landing succeeded!") return True - -def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, - timeout_arm=TIMEOUT_ARMED, timeout_takeoff=TIMEOUT, wait=False, tolerance=TOLERANCE, emergency_land=False, - interrupter=INTERRUPTER): - logger.info("Starting takeoff!") - #print("Starting takeoff!") - logger.info("Arming, going to OFFBOARD mode") - - # Arming check - set_rates(thrust=0.05, auto_arm=True) - telemetry = get_telemetry() - rate = rospy.Rate(freq) +def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, timeout_takeoff=TIMEOUT, interrupter=INTERRUPTER, emergency_land=False): + rospy.loginfo("Takeoff started...") + rate = rospy.Rate(FREQUENCY) + start = get_telemetry(frame_id=frame_id) + climb = 0. time_start = time.time() - - while (not telemetry.armed) or wait: - if interrupter.is_set(): - logger.warning("Takeoff function interrupted!") - #print("Takeoff function interrupted!") - interrupter.clear() - return 'interrupted' - - telemetry = get_telemetry() - logger.info("Arming...") - #print("Arming...") - time_passed = time.time() - time_start - - if timeout_arm is not None: - if time_passed >= timeout_arm: - if not telemetry.armed: - logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed)) - #print('Arming timed out! | time: {:3f} seconds'.format(time_passed)) - return 'not armed' - else: - break - rate.sleep() - - logger.info("Armed!") - #print("Armed") - - # Reach height - z0 = get_telemetry().z - z_dest = z + z0 - navigate(z=z, speed=speed, yaw = float('nan'), frame_id = 'body', auto_arm=True) - current_diff = abs(get_telemetry().z - z_dest) - while (current_diff > tolerance) or wait: + result = navigate(z=height, speed=speed, yaw=float('nan'), frame_id="body", auto_arm=True) + rospy.loginfo(result) + if result.success == False: + return 'not armed' + rospy.logdebug(result) + while abs(climb - height) > tolerance: if interrupter.is_set(): logger.warning("Flight function interrupted!") - #print("Flight function interrupted!") interrupter.clear() return 'interrupted' - current_diff = abs(get_telemetry().z - z_dest) - logger.info("Takeoff...") - #print("Takeoff...") + climb = abs(get_telemetry(frame_id=frame_id).z - start.z) + rospy.logdebug("Takeoff to {:.2f} of {:.2f} meters".format(climb, height)) + time_passed = time.time() - time_start if timeout_takeoff is not None: if time_passed >= timeout_takeoff: - if not wait: - logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed)) - #print('Takeoff timed out! | time: {:3f} seconds'.format(time_passed)) - if emergency_land: - logger.info("Preforming emergency land") - #print("Preforming emergency land") - land(descend=False, interrupter=interrupter) - return 'time out' - else: - break + rospy.loginfo('Takeoff timed out! | time: {:3f} seconds'.format(time_passed)) + if emergency_land: + land(descend=False, interrupter=interrupter) + return 'timeout' + rate.sleep() - - logger.info("Takeoff succeeded!") - #print("Takeoff succeeded!") + rospy.loginfo("Takeoff succeeded!") return 'success' def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different directions diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 6508be5..cb5d681 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -160,14 +160,13 @@ def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True, interrupter=interrupt_event): - print(interrupter.is_set()) if use_leds: LedLib.wipe_to(255, 0, 0, interrupter=interrupter) if interrupter.is_set(): return - result = FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff, - interrupter=interrupter) - if result == 'not armed': + result = FlightLib.takeoff(height=z, timeout_takeoff=timeout, frame_id=frame_id, + emergency_land=safe_takeoff, interrupter=interrupter) + if result == 'not armed' or result == 'timeout': raise Exception('STOP') # Raise exception to clear task_manager if copter can't arm if interrupter.is_set(): return diff --git a/Drone/copter_client.py b/Drone/copter_client.py index ade3a35..bedee2d 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -245,7 +245,7 @@ def _play_animation(**kwargs): "z": client.active_client.TAKEOFF_HEIGHT, "timeout": client.active_client.TAKEOFF_TIME, "safe_takeoff": client.active_client.SAFE_TAKEOFF, - # "frame_id": client.active_client.FRAME_ID, + "frame_id": client.active_client.FRAME_ID, "use_leds": client.active_client.USE_LEDS, } )