Geekly articles weekly

Entrance to Aeronet, Episódio 4: Find and Fuck


Hoje, estamos combinando um programa de controle de drone autônomo com um programa de detecção de bola para explodir a bola com um drone autônomo.


Em artigos anteriores, examinamos o lançamento de um drone virtual e real autônomo, bem como o desenvolvimento de um nó ROS que define e transfere as coordenadas de uma bola no espaço . Agora, refazeremos o programa de controle de drones para que ele tente estourar a bola automaticamente.


Programa voando para a bola


O programa que voa para a bola é baseado no mesmo ciclo de controle de velocidade do programa de controle manual .
A diferença é que, em vez das teclas de controle do teclado, o vetor de velocidade do drone é controlado por informações sobre a posição da bola obtida no tópico /baloon_detector/twist .
setvel_forward velocidades e setvel_forward vector setvel definidos para que o drone voou direto para a bola.
Se o drone não vir a bola por mais de 0,2 segundos, acreditamos que a estouramos e colocamos o drone no modo de pouso.


O texto completo do programa é fornecido abaixo: (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()

Recomendações de depuração


Para depuração, recomendamos escolher um espaço aberto, anexar a bola à base, certificando-se de que não haja objetos vermelhos por perto, para que não haja falsos positivos no programa de orientação.
A operação do programa de orientação, antes do voo, deve ser verificada usando um navegador, em 192.168.11.1:8080:


Se tudo estiver configurado corretamente, o programa deve distinguir com confiança a bola e não causar falsos positivos.


No início do programa de orientação, entende-se que o drone deve ver a bola após a decolagem. Caso contrário, o drone pensará que a bola já estourou e passará para o modo de pouso.
No futuro, você pode modificar o programa - faça uma abordagem à bola, por exemplo, usando o modo AUTO.MISSION. E depois de atingir o ponto GPS desejado, mude para o modo de busca visual.
A seleção de coeficientes de velocidade no ciclo principal do programa é realizada experimentalmente para um drone específico.


Nas competições, as tentativas de rebentar a bola eram assim:



Eu ficaria grato pelos comentários e perguntas daqueles que tentam repetir nosso experimento.


Os códigos-fonte dos programas são carregados no Github .

Source: https://habr.com/ru/post/pt471450/

More articles:


All Articles