import rospy
from clover import srv
from std_srvs.srv import Trigger
import math

rospy.init_node('flight')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
land = rospy.ServiceProxy('land', Trigger)

def wait_arrival(tolerance=0.2):
    while not rospy.is_shutdown():
        telem = get_telemetry(frame_id='navigate_target')
        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
            break
        rospy.sleep(0.2)
    rospy.sleep(2)

# Взлетаем на высоту 2 метра
navigate(x=0, y=0, z=2, frame_id='body', auto_arm=True)
wait_arrival()

# Пролет по квадрату (5 метров на сторону)
navigate(x=5, y=0, z=0, frame_id='body')
wait_arrival()

navigate(x=0, y=5, z=0, frame_id='body')
wait_arrival()

navigate(x=-5, y=0, z=0, frame_id='body')
wait_arrival()

navigate(x=0, y=-5, z=0, frame_id='body')
wait_arrival()

# Возвращение в исходную точку (центральную метку)
navigate(x=0, y=0, z=0, frame_id='body')
wait_arrival()

# Получаем телеметрию и выводим её в консоль
telemetry = get_telemetry()
print(f"Telemetry: x={telemetry.x}, y={telemetry.y}, z={telemetry.z}, yaw={telemetry.yaw}, vx={telemetry.vx}, vy={telemetry.vy}, vz={telemetry.vz}")

# Посадка
land()