waypoint_flight.py

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

# запускаем ros-узел
rospy.init_node('flight_simulation')

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

# пишем телеметрию в файл
def log_telemetry(log_file):
    """запись текущей телеметрии в csv"""
    telemetry = get_telemetry(frame_id='map')
    with open(log_file, 'a', newline='') as csvfile:
        writer = csv.writer(csvfile)
        writer.writerow([time.time(), telemetry.x, telemetry.y, telemetry.z])

# основной сценарий полета
def flight_task(log_file):
    """выполнение полетного задания с записью телеметрии"""
    # создаем или очищаем лог-файл
    with open(log_file, 'w', newline='') as csvfile:
        writer = csv.writer(csvfile)
        writer.writerow(['Time', 'X', 'Y', 'Z'])

    rospy.loginfo("начало выполнения полетного задания")

    # взлет на высоту 4 метра
    rospy.loginfo("взлет до 4 метров")
    navigate(x=0, y=0, z=4, speed=0.5, frame_id='body', auto_arm=True)
    rospy.sleep(5)
    log_telemetry(log_file)

    # полет к точке (5, 5, 5)
    rospy.loginfo("летим в точку (5, 5, 5)")
    navigate(x=5, y=5, z=5, speed=0.5, frame_id='map')
    rospy.sleep(10)
    log_telemetry(log_file)

    # полет к точке (3, 3, 3)
    rospy.loginfo("летим в точку (3, 3, 3)")
    navigate(x=3, y=3, z=3, speed=0.5, frame_id='map')
    rospy.sleep(7)
    log_telemetry(log_file)

    # полет к точке (2, 2, 2)
    rospy.loginfo("летим в точку (2, 2, 2)")
    navigate(x=2, y=2, z=2, speed=0.5, frame_id='map')
    rospy.sleep(5)
    log_telemetry(log_file)

    # возвращаемся в точку (0, 0, 1)
    rospy.loginfo("возвращаемся в точку (0, 0, 1)")
    navigate(x=0, y=0, z=1, speed=0.5, frame_id='map')
    rospy.sleep(7)
    log_telemetry(log_file)

    # посадка
    rospy.loginfo("начинаем посадку")
    land()
    rospy.loginfo("полетное задание выполнено")

# анализ логов
def analyze_logs(output_file):
    """анализ логов телеметрии после полетов с записью итогов в файл"""
    with open(output_file, 'w') as analysis_file:
        for i in range(1, 4):
            log_file = f'flight_log_{i}.csv'
            if not os.path.exists(log_file):
                rospy.logwarn(f"лог-файл {log_file} не найден")
                analysis_file.write(f"лог-файл {log_file} не найден\n")
                continue

            with open(log_file, 'r') as csvfile:
                reader = csv.reader(csvfile)
                next(reader)  # пропускаем заголовок
                positions = [(float(row[1]), float(row[2]), float(row[3])) for row in reader]

            if positions:
                start_pos = positions[0]
                end_pos = positions[-1]
                total_distance = sum(
                    math.dist(positions[j], positions[j-1])
                    for j in range(1, len(positions))
                )
                result = (
                    f"анализ полетного теста {i}:\n"
                    f"  всего записей: {len(positions)}\n"
                    f"  начальная позиция: {start_pos}\n"
                    f"  конечная позиция: {end_pos}\n"
                    f"  общее пройденное расстояние: {total_distance:.2f} метров\n"
                )
                rospy.loginfo(result)
                analysis_file.write(result + "\n")
            else:
                rospy.logwarn(f"в {log_file} нет данных телеметрии")
                analysis_file.write(f"в {log_file} нет данных телеметрии\n")

if __name__ == '__main__':
    try:
        for i in range(1, 4):
            rospy.loginfo(f"начинаем полетный тест {i}")
            flight_task(log_file=f'flight_log_{i}.csv')
            rospy.loginfo(f"полетный тест {i} завершен")
            rospy.sleep(2)  # пауза перед следующим тестом

        # анализ логов после всех тестов
        analyze_logs(output_file='flight_analysis.txt')

    except rospy.ROSInterruptException:
        pass