Geekly articles weekly

Entrée d'Aeronet, épisode 4: Find and Fuck


Aujourd'hui, nous combinons un programme de contrôle de drone autonome avec un programme de détection de balle afin d'éclater la balle avec un drone autonome.


Dans des articles précédents, nous avons examiné le lancement d'un drone virtuel et réel autonome, ainsi que le développement d'un nœud ROS qui définit et transfère les coordonnées d'une balle dans l'espace . Maintenant, nous allons refaire le programme de contrôle des drones pour qu'il essaie automatiquement d'éclater la balle.


Programme volant vers le ballon


Le programme volant vers le ballon est basé sur le même cycle de contrôle de vitesse que dans le programme de contrôle manuel .
La différence est qu'au lieu des touches de contrôle du clavier, le vecteur de vitesse du drone est contrôlé par des informations sur la position de la balle obtenues à partir du sujet /baloon_detector/twist .
setvel_forward vitesses setvel_forward et le vecteur setvel_forward setvel définis de sorte que le drone vole directement dans le ballon.
Si le drone ne voit pas le ballon pendant plus de 0,2 seconde, nous pensons que nous l'avons éclaté et mis le drone en mode d'atterrissage.


Le texte complet du programme est donné ci-dessous: (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()

Recommandations de débogage


Pour le débogage, nous vous recommandons de choisir un espace ouvert, d'attacher la balle à la base, en vous assurant qu'il n'y a pas d'objets rouges à proximité, afin qu'il n'y ait pas de faux positifs du programme de guidage.
Le fonctionnement du programme de guidage, avant le vol, doit être vérifié à l'aide d'un navigateur, au 192.168.11.1:8080:


Si tout est correctement configuré, le programme doit distinguer en toute confiance la balle et ne pas provoquer de faux positifs.


Au début du programme de guidage, il est entendu que le drone devrait voir le ballon après le décollage. Sinon, le drone pensera que la balle a déjà éclaté et passera en mode atterrissage.
À l'avenir, vous pouvez modifier le programme - vous approcher du ballon, par exemple, en utilisant le mode AUTO.MISSION. Et après avoir atteint le point GPS souhaité, passez en mode de recherche visuelle.
La sélection des coefficients de vitesses dans le cycle principal du programme est effectuée expérimentalement pour un drone spécifique.


Lors des compétitions, les tentatives d'éclatement du ballon ressemblaient à ceci:



Je serais reconnaissant pour les commentaires et les questions de ceux qui essaient de répéter notre expérience.


Les codes sources des programmes sont téléchargés sur Github .

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

More articles:


All Articles