diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index dee1d4b..0000000 --- a/.gitmodules +++ /dev/null @@ -1,7 +0,0 @@ -[submodule "Drone/FlightLib"] - path = Drone/FlightLib - url = https://github.com/artem30801/CleverFlightLib.git - #url = https://github.com/goldarte/CleverFlightLib.git -[submodule "blender-csv-animation"] - path = blender-csv-animation - url = https://github.com/artem30801/blender-csv-animation diff --git a/Drone/FlightLib2/FlightLib.py b/Drone/FlightLib2/FlightLib.py deleted file mode 100644 index 7fa797a..0000000 --- a/Drone/FlightLib2/FlightLib.py +++ /dev/null @@ -1,293 +0,0 @@ -#!/usr/bin/python -from __future__ import print_function -import sys -import math -import time -import logging -import threading -import rospy -from clever import srv -from mavros_msgs.srv import SetMode -from mavros_msgs.srv import CommandBool -from std_srvs.srv import Trigger - -module_logger = logging.getLogger("FlightLib.FlightLib") - -# create proxy service -navigate = rospy.ServiceProxy('navigate', srv.Navigate) -set_position = rospy.ServiceProxy('set_position', srv.SetPosition) -set_rates = rospy.ServiceProxy('/set_rates', srv.SetRates) -set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) -get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) -arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) -landing = rospy.ServiceProxy('/land', Trigger) - -module_logger.info("Proxy services inited") - -# globals -FREQUENCY = 1000/25 # HZ -TOLERANCE = 0.2 - -interrupt_event = threading.Event() - - -def interrupt(): - module_logger.info("Performing function interrupt") - interrupt_event.set() - - -def init(node_name="CleverSwarmFlight", anon=True, no_signals=True): - module_logger.info("Initing ROS node") - rospy.init_node(node_name, anonymous=anon, disable_signals=no_signals) - module_logger.info("Ros node inited") - - -def get_distance3d(x1, y1, z1, x2, y2, z2): - return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2) - - -def check(check_name): - def inner(f): - def wrapper(*args, **kwargs): - result, failures = f(*args, **kwargs) - if failures: - msgs = [] - for failure in failures: - msg = "[{}]: Failure: {}".format(check_name, failure) - msgs.append(msg) - module_logger.warning(msg) - return msgs - else: - module_logger.info("[{}]: OK".format(check_name)) - return None - return wrapper - return inner - - -@check("Linear velocity estimation") -def check_linear_speeds(): - failures = [] - telemetry = get_telemetry(frame_id='body') - speed_limit = 0.1 - if telemetry.vx >= speed_limit: - failures.append("X velocity estimation: {:.3f} m/s".format(telemetry.vx)) - if telemetry.vy >= speed_limit: - failures.append("Y velocity estimation: {:.3f} m/s".format(telemetry.vy)) - if telemetry.vz >= speed_limit: - failures.append("Z velocity estimation: {:.3f} m/s".format(telemetry.vz)) - return failures - - -@check("Angular velocity estimation") -def check_angular_speeds(): - failures = [] - telemetry = get_telemetry(frame_id='body') - rate_limit = 0.05 - if telemetry.pitch_rate >= rate_limit: - failures.append("Pitch rate estimation: {:.3f} rad/s".format(telemetry.pitch_rate)) - if telemetry.roll_rate >= rate_limit: - failures.append("Roll rate estimation: {:.3f} rad/s".format(telemetry.roll_rate)) - if telemetry.yaw_rate >= rate_limit: - failures.append("Yaw rate estimation: {:.3f} rad/s".format(telemetry.yaw_rate)) - return failures - - -@check("Angles estimation") -def check_angles(): - failures = [] - telemetry = get_telemetry(frame_id='body') - angle_limit = math.radians(1) - if abs(telemetry.pitch) >= angle_limit: - failures.append("Pitch estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.pitch, - math.degrees(telemetry.pitch))) - if abs(telemetry.roll) >= angle_limit: - failures.append("Roll estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.roll, - math.degrees(telemetry.roll))) - if abs(telemetry.yaw) >= angle_limit: - failures.append("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw, - math.degrees(telemetry.yaw))) - return failures - - -def selfcheck(): - msgs = [] - msgs.extend(check_linear_speeds()) - msgs.extend(check_angular_speeds()) - msgs.extend(check_angles()) - - return msgs - - -def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map'): - set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw) - telemetry = get_telemetry(frame_id=frame_id) - - module_logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) - module_logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z)) - - return True - - -def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map', - freq=FREQUENCY, timeout=5000, wait=False): - - module_logger.info('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 - telemetry = get_telemetry(frame_id=frame_id) - rate = rospy.Rate(freq) - time_start = rospy.get_rostime() - - while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - break - - telemetry = get_telemetry(frame_id=frame_id) - module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( - telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) - - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 - - if timeout is not None: - if time_passed >= timeout: - module_logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed / 1000)) - return wait - rate.sleep() - else: - module_logger.info("Point reached!") - return True - - -def reach_attitude(z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map', - freq=FREQUENCY, timeout=5000, wait=False): - - module_logger.info('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) - - # waiting for completion - telemetry = get_telemetry(frame_id=frame_id) - rate = rospy.Rate(freq) - time_start = rospy.get_rostime() - - while (abs(z - telemetry.z) > tolerance) or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - break - - telemetry = get_telemetry(frame_id=frame_id) - module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( - telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) - - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 - if timeout is not None: - if time_passed >= timeout: - module_logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed / 1000)) - return wait - rate.sleep() - else: - module_logger.info("Attitude reached!") - return True - - -def land(descend=True, z=1.0, frame_id_descend="aruco_map", frame_id_land="aruco_map", - timeout_descend=5000, timeout_land=7500, freq=FREQUENCY): - if descend: - module_logger.info("Descending to: | z: {:.3f}".format(z)) - reach_attitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=1.57) #TODO yaw - landing() - - telemetry = get_telemetry(frame_id='aruco_map') - rate = rospy.Rate(freq) - time_start = rospy.get_rostime() - - while telemetry.armed: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - break - - telemetry = get_telemetry(frame_id=frame_id_land) - module_logger.info("Landing...") - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 - - if timeout_land is not None: - if time_passed >= timeout_land: - module_logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed / 1000)) - module_logger.warning("Disarming!") - arming(False) - return False - rate.sleep() - else: - module_logger.info("Landing succeeded!") - return True - - -def takeoff(z=1.0, speed=0.8, frame_id='body', freq=FREQUENCY, - timeout_arm=2000, timeout_takeoff=5000, wait=False, tolerance=TOLERANCE, emergency_land=False): - module_logger.info("Starting takeoff!") - print("Starting takeoff!") - module_logger.info("Arming, going to OFFBOARD mode") - - # Arming check - set_rates(thrust=0.1, auto_arm=True) - telemetry = get_telemetry(frame_id=frame_id) - rate = rospy.Rate(freq) - time_start = rospy.get_rostime() - - while (not telemetry.armed) or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - return None - - telemetry = get_telemetry(frame_id=frame_id) - module_logger.info("Arming...") - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 - - if timeout_arm is not None: - if time_passed >= timeout_arm: - if not telemetry.armed: - module_logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000)) - return False - else: - break - rate.sleep() - - module_logger.info("Armed!") - print("Armed!") - - # Reach height - telemetry = get_telemetry(frame_id=frame_id) - z0 = get_telemetry().z - navigate(z=z, speed=speed, frame_id=frame_id, auto_arm=True) - current_height = abs(get_telemetry().z - z0 - z) - while current_height > tolerance or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - return None - - current_height = abs(get_telemetry().z - z0 - z) - module_logger.info("Takeoff...") - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 - - if timeout_takeoff is not None: - if time_passed >= timeout_takeoff: - if not wait: - module_logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed / 1000)) - if emergency_land: - module_logger.info("Preforming emergency land") - land(descend=False) - return False - else: - break - rate.sleep() - - module_logger.info("Takeoff succeeded!") - print("Takeoff succeeded!") - return True diff --git a/Drone/FlightLib2/__init__.py b/Drone/FlightLib2/__init__.py deleted file mode 100644 index 8b13789..0000000 --- a/Drone/FlightLib2/__init__.py +++ /dev/null @@ -1 +0,0 @@ - diff --git a/blender-csv-animation b/blender-csv-animation deleted file mode 160000 index e02ffe3..0000000 --- a/blender-csv-animation +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e02ffe33299bea5f8cdff53ce0143235208d9ecf