---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

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

rospy.init_node('clover_autonomous_mission')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
land = rospy.ServiceProxy('land', Trigger)
set_effect = rospy.ServiceProxy('led/set_effect', srv.SetLEDEffect)

try:
    set_effect(effect='fill', r=255, g=255, b=255)
    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='body', auto_arm=True)
    rospy.sleep(5.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_42')
    rospy.sleep(4.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_44')
    rospy.sleep(4.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_38')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=128, g=0, b=128)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_48')
    rospy.sleep(5.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_41')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=0, g=191, b=255)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_27')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=128, g=0, b=128)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_13')
    rospy.sleep(4.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_12')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=255, g=255, b=255)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_11')
    rospy.sleep(4.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_17')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=255, g=0, b=0)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_9')
    rospy.sleep(4.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_8')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=0, g=255, b=0)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_0')
    rospy.sleep(5.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_7')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=255, g=255, b=0)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_21')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=0, g=0, b=255)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_35')
    rospy.sleep(4.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_38')
    rospy.sleep(4.0)
    set_effect(effect='fill', r=128, g=0, b=128)
    rospy.sleep(1.0)

    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='aruco_42')
    rospy.sleep(5.0)

    land()
    rospy.sleep(4.0)
    set_effect(effect='fill', r=0, g=0, b=0)

except rospy.ROSInterruptException:
    pass