View difference between Paste ID: gWmKyfk5 and 8e0Hn0X6

-#!/usr/bin/env python
+
-# -*- coding: utf-8 -*-
+
 from std_srvs.srv import Trigger
 import math
 
 rospy.init_node('flight')
 
-import csv
+
 navigate = rospy.ServiceProxy('navigate', srv.Navigate)
 land = rospy.ServiceProxy('land', Trigger)
 
-# Define service proxies
+def wait_arrival(tolerance=0.2):
     while not rospy.is_shutdown():
         telem = get_telemetry(frame_id='navigate_target')
         if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
             break
-def wait_arrival(tolerance=0.2, debug=False, log_writer=None):
+        rospy.sleep(0.2)
-    """
+
-    Waits until the drone reaches the target position within a certain tolerance.
+
-    Logs telemetry data to the provided CSV writer during the wait.
+# Взлетаем на высоту 2 метра
-    """
+navigate(x=0, y=0, z=2, frame_id='body', auto_arm=True)
-    rate = rospy.Rate(5)  # 5 Hz
+wait_arrival()
 
 # Пролет по квадрату (5 метров на сторону)
-        distance = math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2)
+navigate(x=5, y=0, z=0, frame_id='body')
-
+wait_arrival()
-        if debug:
+
-            rospy.loginfo(f"Current distance to target: {distance}")
+navigate(x=0, y=5, z=0, frame_id='body')
-
+wait_arrival()
-        if log_writer:
+
-            global_telem = get_telemetry(frame_id='map')
+navigate(x=-5, y=0, z=0, frame_id='body')
-            log_writer.writerow([rospy.get_time(), global_telem.x, global_telem.y, global_telem.z, global_telem.yaw])
+wait_arrival()
-
+
-        if distance < tolerance:
+navigate(x=0, y=-5, z=0, frame_id='body')
 wait_arrival()
-        rate.sleep()
+
 # Возвращение в исходную точку (центральную метку)
 navigate(x=0, y=0, z=0, frame_id='body')
-def navigate_and_wait(x, y, z, frame_id='body', auto_arm=False, debug=False, log_writer=None):
+wait_arrival()
-    """
+
-    Sends the drone to the specified coordinates and waits until arrival.
+# Получаем телеметрию и выводим её в консоль
-    """
+telemetry = get_telemetry()
-    navigate(x=x, y=y, z=z, frame_id=frame_id, auto_arm=auto_arm)
+print(f"Telemetry: x={telemetry.x}, y={telemetry.y}, z={telemetry.z}, yaw={telemetry.yaw}, vx={telemetry.vx}, vy={telemetry.vy}, vz={telemetry.vz}")
-    if debug:
+
-        rospy.loginfo(f"Navigating to: x={x}, y={y}, z={z}, frame_id={frame_id}")
+# Посадка
-    wait_arrival(debug=debug, log_writer=log_writer)
+land()