flight

#!/usr/bin/env python3
import rospy
from clover import srv
from std_srvs.srv import Trigger
from math import sqrt

# Инициализация ROS-узла
rospy.init_node('flight_mission')

# Подключение к сервисам Clover
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
navigate_wait = rospy.ServiceProxy('navigate_wait', Trigger)
land = rospy.ServiceProxy('land', Trigger)

def wait_arrival():
    """Ожидает, пока дрон не достигнет заданной позиции."""
    rospy.wait_for_service('navigate_wait')
    try:
        res = navigate_wait()
        if not res.success:
            rospy.logerr("Navigation failed!")
    except rospy.ServiceException as e:
        rospy.logerr(f"Service call failed: {e}")

# Получение стартовой позиции
rospy.loginfo("Getting start position...")
telemetry_start = get_telemetry(frame_id='map')
start_x = telemetry_start.x
start_y = telemetry_start.y
start_z = telemetry_start.z
rospy.loginfo(f"Start position recorded: x={start_x}, y={start_y}, z={start_z}")

# [1] Взлет на высоту 1 метр
rospy.loginfo("Taking off to 1 meter...")
navigate(x=0, y=0, z=1, yaw=float('nan'), frame_id='body', auto_arm=True)
wait_arrival()

# [2] Полет в точку (5, 5, 5)
rospy.loginfo("Flying to point (5, 5, 5)...")
navigate(x=5, y=5, z=5, yaw=float('nan'), frame_id='map')
wait_arrival()

# [3] Полет в точку (3, 3, 3)
rospy.loginfo("Flying to point (3, 3, 3)...")
navigate(x=3, y=3, z=3, yaw=float('nan'), frame_id='map')
wait_arrival()

# [4] Возврат к стартовой точке
rospy.loginfo("Returning to start position...")
navigate(x=start_x, y=start_y, z=1, yaw=float('nan'), frame_id='map')
wait_arrival()

# Посадка
rospy.loginfo("Landing...")
land()
rospy.loginfo("Mission completed successfully.")