fly.py

# Подключаем стандартные библиотеки
import sys

# Подключаем библиотеки ROS  
import rospy
import mavros
from mavros import command
from mavros_msgs.msg import PositionTarget
from geometry_msgs.msg import Point, Vector3
from sensor_msgs.msg import LaserScan


# Какая-то инициализация mavros, не знаю что именно делает
mavros.set_namespace()

# Включаем арминг
command.arming(True)

class Fly(object):
    # Функция __init__ это т.н. "конструктор", вызовется когда мы создадим объект Fly (т.е. сделаем Fly())
    def __init__(self):
        # Создаем ROS ноду(=некое подобие пользователя) с именем aerobot (произвольное имя)
        rospy.init_node('aerobot', anonymous=True)

        # Создаем publisher(ближайший перевод - отправитель) для топика управления полетом /mavros/setpoint_raw/local 
        self.pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)
        # Создаем subscriber(ближайший перевод - подписчик) для топика дальномера /sf30/range
        # Как только в топик поступит новое сообщение - будет вызвана ф-ция on_alt_upd, куда это новое сообщение будет отправленно в виде параметра
        rospy.Subscriber('/sf30/range', LaserScan, self.on_alt_upd)
        # Создаем таймер, который будет использоваться для засыпания на 1/10 секунды = мы будем контролировать полет с частотой 10hz
        self.rate = rospy.Rate(10) # 10hz

        # Создаем переменную для хранения последнего значения высоты
        self.alt = 0

    # Эта ф-ция вызывается на каждое обновление данных с дальномера и сохраняет полученное значение в self.alt
    def on_alt_upd(self, msg):
        self.alt = msg.ranges[0]

    # Эта ф-ция запускает основной цикл программы, в котором выполняется полет
    def run(self):
        # Цикл, пока rospy работает
        while not rospy.is_shutdown():
            # Разница между нужной высотой (0.5) и последним сохраненным значением с дальномера (self.alt) 
            alt_diff = 0.5 - self.alt
            # Вертикальная скорость, которая будет установлена
            v = 1 + (alt_diff / 0.5) * 2

            # Эти 2 строчки просто выводят всю информацию в терминал 
            # Пример: alt: 0.17 alt diff: 0.33 v: 2.34
            # Почему код выглядит так сложно: сделано ограничение точности всех чисел до 2х знаков (это 'round(self.alt, 2)') и фиксация на экране (это \r в конце) 
            sys.stdout.write('alt: ' + str(round(self.alt,2)) + ' alt diff: ' + str(round(alt_diff,2)) + ' v: ' + str(round(v,2)) + '\r')
            sys.stdout.flush()

            # Создаем сообщение для публикации в топик /mavros/setpoint_raw/local
            msg = PositionTarget(
                velocity=Vector3(x=0, y=0, z=v if self.alt < 0.5 else 0), # Нужная нам скорость, по z принимает 0 если мы выше 0.5 или v если ниже 
                coordinate_frame=PositionTarget.FRAME_LOCAL_NED, # не знаю
                type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + # через данный канал можно управлять ещё многими полетными параметрами - указываем, что их у нас нет и нужно их игнорировать
                    PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + \
                    PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
            )
            # Отправляем сообщение в топик
            self.pub.publish(msg)

            # Спим интервал 10Hz до следующего обновления
            self.rate.sleep()

# Создаем объект Fly и вызываем его ф-цию run
Fly().run()