fly_nocomment.py

import sys

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.set_namespace()
command.arming(True)

class Fly(object):
    def __init__(self):
        rospy.init_node('aerobot', anonymous=True)

        self.pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)
        rospy.Subscriber('/sf30/range', LaserScan, self.on_alt_upd)
        self.rate = rospy.Rate(10) # 10hz

        self.alt = 0

    def on_alt_upd(self, msg):
        self.alt = msg.ranges[0]

    def run(self):
        while not rospy.is_shutdown():
            alt_diff = 0.5 - self.alt
            v = 1 + (alt_diff / 0.5) * 2

            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()

            msg = PositionTarget(
                velocity=Vector3(x=0, y=0, z=v if self.alt < 0.5 else 0), 
                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)

            self.rate.sleep()

Fly().run()