Сегодня мы объединяем программу управления автономным дроном с программой обнаружения шарика, дабы лопнуть уже шарик автономным дроном.


В предыдущих статьях мы рассматривали запуск автономного виртуального и реального дрона, а также разработку ROS-ноды, определяющей и передающей координаты шарика в пространстве. Теперь переделаем программу управления дроном так, чтобы он автоматически стремился лопнуть шарик.


Программа, летящая к шарику


В основе программы, летящей к шарику – тот же цикл управления скоростями, что и в программе ручного управления.
Отличие – в том, что вместо клавиш управления с клавиатуры вектором скоростей дрона управляет информация о положении шарика, полученная из топика /baloon_detector/twist.
Скорости setvel_forward и вектор setvel устанавливаются таким образом, чтоб дрон летел прямо в шарик.
Если дрон не видит шарик более чем 0.2 секунды – считаем, что мы его лопнули, и переводим дрон в режим посадки.


Полный текст программы приведён ниже: (crash_baloon.py)
#!/usr/bin/env python
# coding=UTF-8
# baloon position steering
import rospy
import mavros
import mavros.command as mc
from mavros_msgs.msg import State
from geometry_msgs.msg import PoseStamped, Twist, Quaternion, TwistStamped
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import SetMode
import tf.transformations as t
import math

current_state=State()
current_pose = PoseStamped()
current_vel = Twist()
baloon_twist = TwistStamped()

def state_callback(data):
    global current_state
    current_state=data

def localpose_callback(data):
    global current_pose
    current_pose = data

def baloon_callback(data):
    global baloon_twist
    baloon_twist = data

def publish_setvel(event):
    global current_pose, setvel_pub, setvel, setvel_forward, baloon_twist
    q=current_pose.pose.orientation.x, current_pose.pose.orientation.y,current_pose.pose.orientation.z,current_pose.pose.orientation.w
    roll, pitch, yaw = t.euler_from_quaternion(q)
    setvel.linear.x = setvel_forward * math.cos(yaw)
    setvel.linear.y = setvel_forward * math.sin(yaw)
    setvel_pub.publish(setvel)

def main():
    global current_pose, setvel, setvel_pub, setvel_forward, baloon_twist

    rospy.init_node("offbrd",anonymous=True)
    rate=rospy.Rate(10)
    state=rospy.Subscriber("/mavros/state",State,state_callback)
    pose_sub=rospy.Subscriber("/mavros/local_position/pose",PoseStamped,localpose_callback)
    baloon_sub=rospy.Subscriber("/baloon_detector/twist",TwistStamped,baloon_callback)
    setvel_pub=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1)
    arming_s=rospy.ServiceProxy("/mavros/cmd/arming",CommandBool)
    set_mode=rospy.ServiceProxy("/mavros/set_mode",SetMode)
    setvel=Twist()
    setvel_forward = 0

    arming_s(True)
    set_mode(0,"AUTO.TAKEOFF")
    print 'Taking off.....\r'
    rospy.sleep(5)

    for i in range (0,10):
        setvel_pub.publish(setvel)
        rate.sleep()
    set_mode(0,"OFFBOARD")

    setvel_timer = rospy.Timer(rospy.Duration(0.05), publish_setvel)

    while not rospy.is_shutdown():
        time_delay = rospy.Time.now().to_sec() - baloon_twist.header.stamp.to_sec()
        #print baloon_twist
        print 'time delay = ',time_delay
        if time_delay<0.2:#последний раз шарик видели 0.2 секунды назад 
            if baloon_twist.twist.linear.x > 0.8:
                setvel_forward = 1.5
            elif baloon_twist.twist.linear.x > 0.8:
                setvel_forward = 0.0
            else:
                setvel_forward = -0.5
            setvel.angular.z = baloon_twist.twist.angular.z*4
            if baloon_twist.twist.angular.y<0:
                setvel.linear.z=0.5
            elif baloon_twist.twist.angular.y>0.2:
                setvel.linear.z=-0.25
            else:
                setvel.linear.z=0
        else:#шарик потерян из виду
            setvel.angular.z=setvel_forward=setvel.linear.z=0
        print setvel, setvel_forward
        rate.sleep()

    set_mode(0,"AUTO.LAND")
    print 'Landing.......\r'
    setvel_timer.shutdown()
    rospy.sleep(5)

if __name__=="__main__":
    main()

Рекомендации по отладке


Для отладки рекомендуем выбирать открытое пространство, прикрепить шарик к основанию, убедиться, что рядом нет красных предметов, чтобы не было ложных срабатываний программы наведения.
Работу программы наведения, до полёта, следует проверить с помощью браузера, по адресу 192.168.11.1:8080:


Если всё настроено правильно – программа должна уверенно различать шарик и не вызывать ложных срабатываний.


При старте программы наведения подразумевается, что дрон должен увидеть шарик после взлёта. Иначе дрон подумает, что шарик уже лопнул, и перейдёт в режим посадки.
В дальнейшем можно модифицировать программу – осуществить подлёт к шарику, например, с помощью режима AUTO.MISSION. И уже после достижения нужной GPS-точки перейти в режим визуального поиска.
Подбор коэффициентов для скоростей в основном цикле программы осуществляется экспериментальным образом для конкретного дрона.


На соревнованиях попытки лопанья шарика выглядели так:



Буду благодарен за комментарии и вопросы от тех, кто попытается повторить наш эксперимент.


Исходные коды программ выложены на Гитхабе.

Комментарии (2)


  1. ghrb
    15.10.2019 10:22

    А за подготовку к терракту вас не могут подтянуть? А то это же прямая попытка сделать дрон самонаводящийся на вертолёт например. Сейчас и за меньшее лет на 6 шесть можно присесть влёгкую.


  1. fmkit
    15.10.2019 11:36

    Лазертаг удобнее чем расход шариков, пару недель назад пробовал самодельный коптер с лазером, сейчас вообще отлично работает но видео пока нет