From 1e6e1328d6c9137d61d8a8448eb3de4cd85df5d2 Mon Sep 17 00:00:00 2001 From: Leonid Rogov Date: Fri, 14 Jun 2019 17:12:31 +0300 Subject: [PATCH] Add flip function, remove debug level of ros logger --- Drone/FlightLib/FlightLib.py | 94 +++++++++++++++++++++++------------- Drone/copter_client.py | 28 ++++++++--- 2 files changed, 80 insertions(+), 42 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 723e4a6..334caeb 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -28,7 +28,7 @@ logger.info("Proxy services inited") FREQUENCY = 40 # HZ TOLERANCE = 0.2 SPEED = 1.0 -SPEED_TAKEOFF = 0.8 +SPEED_TAKEOFF = 1.0 TIMEOUT = 5.0 TIMEOUT_ARMED = 2.0 TIMEOUT_DESCEND = TIMEOUT @@ -37,6 +37,7 @@ Z_DESCEND = 0.5 Z_TAKEOFF = 1.0 FRAME_ID = 'map' INTERRUPTER = threading.Event() +FLIP_MIN_Z = 2.0 checklist = [] @@ -65,7 +66,7 @@ def check(check_name): print(failures) msgs = [] for failure in failures: - print(failure) + #print(failure) msg = "[{}]: Failure: {}".format(check_name, failure) msgs.append(msg) logger.warning(msg) @@ -154,9 +155,9 @@ def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, **kwargs): set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw) #telemetry = get_telemetry(frame_id=frame_id) - #logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) #print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) - #logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z)) + ##logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z)) #print('Telemetry now: | z: {:.3f}'.format(telemetry.z)) return True @@ -165,7 +166,7 @@ def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, **kwargs): def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False): logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) - print('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + #print('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed) # waiting for completion @@ -176,19 +177,19 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait: if interrupter.is_set(): logger.warning("Reach point function interrupted!") - print("Reach point function interrupted!") + #print("Reach point function interrupted!") interrupter.clear() return False telemetry = get_telemetry(frame_id=frame_id) logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) - print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( - telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) + #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + # telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) logger.info('Current delta: | {:.3f}'.format( get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) - print('Current delta: | {:.3f}'.format( - get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) + #print('Current delta: | {:.3f}'.format( + # get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) time_passed = time.time() - time_start @@ -200,14 +201,14 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO rate.sleep() logger.info("Point reached!") - print("Point reached!") + #print("Point reached!") return True def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False): logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) - print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) + #print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) current_telem = get_telemetry(frame_id=frame_id) navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed) @@ -218,26 +219,26 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr while (abs(z - telemetry.z) > tolerance) or wait: if interrupter.is_set(): logger.warning("Reach altitude function interrupted!") - print("Reach altitude function interrupted!") + #print("Reach altitude function interrupted!") interrupter.clear() return telemetry = get_telemetry(frame_id=frame_id) logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) - print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( - telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) + #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + # telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) time_passed = time.time() - time_start if timeout is not None: if time_passed >= timeout: logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed)) - print('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed)) + #print('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed)) return wait rate.sleep() logger.info("Altitude reached!") - print("Altitude reached!") + #print("Altitude reached!") return True @@ -249,11 +250,11 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA timeout_descend=TIMEOUT_DESCEND, timeout_land=TIMEOUT_LAND, freq=FREQUENCY, interrupter=INTERRUPTER): if descend: logger.info("Descending to: | z: {:.3f}".format(z)) - print("Descending to: | z: {:.3f}".format(z)) + #print("Descending to: | z: {:.3f}".format(z)) reach_altitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=float('nan'), # TODO yaw interrupter=interrupter) landing() - print("Land is started") + #print("Land is started") telemetry = get_telemetry(frame_id=frame_id_land) rate = rospy.Rate(freq) @@ -262,26 +263,26 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA while telemetry.armed: if interrupter.is_set(): logger.warning("Land function interrupted!") - print("Land function interrupted!") + #print("Land function interrupted!") interrupter.clear() return False telemetry = get_telemetry(frame_id=frame_id_land) logger.info("Landing... | z: {}".format(telemetry.z)) - print("Landing... | z: {}".format(telemetry.z)) + #print("Landing... | z: {}".format(telemetry.z)) time_passed = time.time() - time_start if timeout_land is not None: if time_passed >= timeout_land: logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed)) logger.warning("Disarming!") - print("Landing timed out, disarming!!!") + #print("Landing timed out, disarming!!!") arming(False) return False rate.sleep() logger.info("Landing succeeded!") - print("Landing succeeded!") + #print("Landing succeeded!") return True @@ -289,7 +290,7 @@ 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!") + #print("Starting takeoff!") logger.info("Arming, going to OFFBOARD mode") # Arming check @@ -301,53 +302,53 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, while (not telemetry.armed) or wait: if interrupter.is_set(): logger.warning("Takeoff function interrupted!") - print("Takeoff function interrupted!") + #print("Takeoff function interrupted!") interrupter.clear() return 'interrupted' telemetry = get_telemetry() logger.info("Arming...") - print("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)) + #print('Arming timed out! | time: {:3f} seconds'.format(time_passed)) return 'not armed' else: break rate.sleep() logger.info("Armed!") - print("Armed") + #print("Armed") # Reach height z0 = get_telemetry().z z_dest = z + z0 - navigate(z=z, speed=speed, yaw = float('nan'), auto_arm=True) + 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: if interrupter.is_set(): logger.warning("Flight function interrupted!") - print("Flight function interrupted!") + #print("Flight function interrupted!") interrupter.clear() return 'interrupted' current_diff = abs(get_telemetry().z - z_dest) logger.info("Takeoff...") - print("Takeoff...") + #print("Takeoff...") 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)) + #print('Takeoff timed out! | time: {:3f} seconds'.format(time_passed)) if emergency_land: logger.info("Preforming emergency land") - print("Preforming emergency land") + #print("Preforming emergency land") land(descend=False, interrupter=interrupter) return 'time out' else: @@ -355,5 +356,30 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, rate.sleep() logger.info("Takeoff succeeded!") - print("Takeoff succeeded!") + #print("Takeoff succeeded!") return 'success' + +def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions + logger.info("Flip started!") + + start_telemetry = get_telemetry() # memorize starting position + + if start_telemetry.z < min_z - TOLERANCE: + logger.warning("Can't do flip! Flip failed!") + #print("Can't do flip! Flip failed!") + else: + # Flip! + set_rates(thrust=1) # bump up + rospy.sleep(0.2) + + set_rates(roll_rate=30, thrust=0.2) # maximum roll rate + + while True: + telem = get_telemetry() + + if -math.pi + 0.1 < telem.roll < -0.2: + break + + logger.info('Flip succeeded!') + #print('Flip succeeded!') + navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw) # finish flip diff --git a/Drone/copter_client.py b/Drone/copter_client.py index a362057..6f7cb96 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -12,7 +12,16 @@ import messaging_lib as messaging import tasking_lib as tasking import animation_lib as animation -import ros_logging +logging.basicConfig( # TODO all prints as logs + level=logging.DEBUG, # INFO + format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", + handlers=[ + logging.StreamHandler(), + ]) + +logger = logging.getLogger(__name__) + +#import ros_logging class CopterClient(client.Client): def load_config(self): @@ -28,14 +37,14 @@ class CopterClient(client.Client): self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') self.X0 = self.config.getfloat('PRIVATE', 'x0') self.Y0 = self.config.getfloat('PRIVATE', 'y0') - self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') + self.LED_PIN = self.config.getint('PRIVATE', 'led_pin') def start(self, task_manager_instance): client.logger.info("Init ROS node") - rospy.init_node('Swarm_client', anonymous=True, log_level=rospy.DEBUG) + rospy.init_node('Swarm_client', anonymous=True) if self.USE_LEDS: - LedLib.init_led() + LedLib.init_led(self.LED_PIN) task_manager_instance.start() @@ -75,6 +84,9 @@ def _command_led_test(*args, **kwargs): time.sleep(2) LedLib.off() +@messaging.message_callback("flip") +def _copter_flip(): + FlightLib.flip() @messaging.message_callback("takeoff") def _command_takeoff(**kwargs): @@ -193,8 +205,8 @@ if __name__ == "__main__": copter_client.start(task_manager) - ros_logging.route_logger_to_ros() - ros_logging.route_logger_to_ros("__main__") - ros_logging.route_logger_to_ros("client") - ros_logging.route_logger_to_ros("messaging") + #ros_logging.route_logger_to_ros() + #ros_logging.route_logger_to_ros("__main__") + #ros_logging.route_logger_to_ros("client") + #ros_logging.route_logger_to_ros("messaging")