进入航空网,第4集:寻找与操弄


今天,我们将自主无人机控制程序球检测程序结合在一起,以便利用自主无人机使球爆裂。


在之前的文章中,我们研究了自主虚拟真实无人机的发射,以及定义和传递太空球坐标的ROS节点的开发。 现在,我们将重新制作无人机控制程序,使其自动尝试使球破裂。


程序飞舞


飞向球的程序是基于与手动控制程序相同的速度控制周期的。
区别在于,无人机速度矢量不是由键盘上的控制键来控制,而是由从主题/baloon_detector/twist获得的有关球位置的信息控制。
setvel_forward速度和setvel_forward矢量,以使无人驾驶飞机向球右飞。
如果无人机在超过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点后,切换到视觉搜索模式。
在程序主循环中选择速度的系数是针对特定无人机通过实验进行的。


在比赛中,尝试将球炸开如下:



对于那些试图重复我们的实验的人的评论和问题,我将不胜感激。


程序的源代码已上传到Github

Source: https://habr.com/ru/post/zh-CN471450/


All Articles