---

import rospy
from clover import srv
from std_srvs.srv import Trigger
from led_msgs.srv import SetLEDBounded

rospy.init_node('flight_mission')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
land = rospy.ServiceProxy('land', Trigger)
set_led = rospy.ServiceProxy('led/set_bounded', SetLEDBounded)

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

navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)
rospy.sleep(3)
set_led(r=255, g=255, b=255)

navigate(x=2, y=0, z=1, speed=0.5, frame_id='map')
wait_arrival()

navigate(x=3, y=1, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=255, g=0, b=255)

navigate(x=5, y=2.5, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=255, g=255, b=0)

navigate(x=6, y=3, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=0, g=255, b=255)

navigate(x=5, y=5, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=128, g=0, b=128)

navigate(x=4, y=5, z=1, speed=0.5, frame_id='map')
wait_arrival()

navigate(x=3, y=4, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=255, g=0, b=0)

navigate(x=2, y=5, z=1, speed=0.5, frame_id='map')
wait_arrival()

navigate(x=1, y=5, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=255, g=255, b=0)

navigate(x=1, y=3, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=0, g=255, b=0)

navigate(x=2, y=2.5, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=0, g=0, b=255)

navigate(x=3, y=1, z=1, speed=0.5, frame_id='map')
wait_arrival()
set_led(r=255, g=0, b=255)

navigate(x=0, y=0, z=1, speed=0.5, frame_id='map')
wait_arrival()

land()
rospy.sleep(5)
set_led(r=0, g=0, b=0)