#!/usr/bin/env python
# استيراد الوحدات اللازمة
import rospy
from mavros_msgs.msg import State
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Bool
# حالة الطائرة
current_state = None
# دالة رد نداء الحالة
def state_cb(state):
global current_state
current_state = state
# دالة رئيسية
def main():
# تهيئة العقدة
rospy.init_node('drone_control', anonymous=True)
# مشترك الحالة
state_sub = rospy.Subscriber('/mavros/state', State, state_cb)
# ناشر الموقف
local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
# ناشر وضع الطيران
arming_pub = rospy.Publisher('/mavros/cmd/arming', Bool, queue_size=1)
# تحديد معدل النشر
rate = rospy.Rate(20)
# انتظار الاتصال
while not current_state:
rate.sleep()
# تحديد الموقف الأولي
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 1
# الإقلاع
while not current_state.armed:
arming_pub.publish(True)
local_pos_pub.publish(pose)
rate.sleep()
# التحليق
while not rospy.is_shutdown():
local_pos_pub.publish(pose)
rate.sleep()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass
تعليقات
إرسال تعليق