docs

import rospy
from aruco_pose.msg import MarkerArray
from clover import srv
from std_srvs.srv import Trigger
from clover.srv import SetLEDEffect
import math

rospy.init_node('my_node')

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', SetLEDEffect)

def navigate_wait(x=0, y=0, z=0, speed=0.5, frame_id='body', auto_arm=False):
    navigate(x=x, y=y, z=z, yaw=float('nan'), speed=speed, frame_id=frame_id, auto_arm=auto_arm)

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

marker_detected = False

def markers_callback(msg):
    global marker_detected
    if marker_detected:
        return

    for marker in msg.markers:
        if marker.id == 101: 
            marker_detected = True
            print(f"Marker {marker.id} detected!")
            set_effect(r=0, g=0, b=255) 
            
            for o in range(9, 1, -2): 
                navigate_wait(x=0, y=0, z=o/10.0, speed=1, frame_id=f'aruco_{marker.id}')
            land()
            set_effect(r=0, g=255, b=0)
            rospy.sleep(3)
            break

navigate_wait(z=1, frame_id='body', auto_arm=True)
rospy.sleep(2)

rospy.Subscriber('aruco_detect/markers', MarkerArray, markers_callback)

g = 0
rate = rospy.Rate(10)
while not marker_detected:
    navigate_wait(z=1, frame_id=f'aruco_{g}')
    g += 1
    rate.sleep()

rospy.spin()