При копировании копируется код целиком

public/shablons/README.md

@@ -0,0 +1,13 @@
+# В данной папке хранятся шаблоны кода для автономного полёта:
+
+## 1. emergency_and_navigate.py
+
+### Указаны все необходимые импорты, также функция перехвата Ctrl+C (signal_handler), безопасной посадки (emergency_stop), navigate_wait
+
+## 2. color.py
+
+### Функция определения цвета платформы. Также показано, как вызывать данную функцию (через подписчика (Subscriber))
+
+## 3. qr.py
+
+### Функция считывания и расшифровки qr кода. Также показано, как вызывать данную функцию (через подписчика (Subscriber))

public/shablons/color.txt

@@ -0,0 +1,54 @@
+class ColorDetect():
+    def __init__(self):
+        self.color = None
+
+    def getColor(self):
+        return self.color
+
+    def detect_color_callback(self, data, print_type='str'):
+        '''определение цвета'''
+
+        frame = bridge.imgmsg_to_cv2(data, 'bgr8')
+        height, width = frame.shape[:2]
+        y_start = int(height * 0.33)
+        y_end = int(height * 0.67)
+        x_start = int(width * 0.31)
+        x_end = int(width * 0.69)
+
+        cropped_frame = frame[y_start:y_end, x_start:x_end]
+        
+        if cropped_frame.size == 0:
+            return
+        
+
+        if print_type == 'rgb':
+
+            mean_bgr = cv2.mean(cropped_frame)[:3]
+            mean_rgb = (int(mean_bgr[2]), int(mean_bgr[1]), int(mean_bgr[0]))  
+            self.color = str(mean_rgb)
+        
+
+        elif print_type == 'str':
+
+            # Получаем средние значения BGR
+            mean_bgr = cv2.mean(cropped_frame)[:3]
+            b, g, r = int(mean_bgr[0]), int(mean_bgr[1]), int(mean_bgr[2])
+            
+            
+            if b > g and b > r and b > 100:  
+                self.color = 'blue'
+            elif g > b and g > r and g > 100:  
+                self.color = 'green'
+            elif r > b and r > g and r > 100:  
+                self.color = 'red'
+            else:
+                self.color = None
+
+#в MAIN для вызова функции должно быть:
+# cd = ColorDetect()
+    #color_sub = rospy.Subscriber('main_camera/image_raw', Image, 
+                              #lambda img: cd.detect_color_callback(img, print_type='rgb'), 
+                              #queue_size=1)
+    # print(cd.getColor())
+    #rospy.sleep(2)
+    #color_sub.unregister()

public/shablons/emergency_and_navigate.txt

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+
+import rospy
+import time
+import signal
+import sys
+from clover import srv
+from std_srvs.srv import Trigger
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+from pyzbar import pyzbar
+import numpy as np
+import math
+import cv2
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) #сеовис для получения местонахождения
+navigate = rospy.ServiceProxy('navigate', srv.Navigate) #сервис для полета по указаным координатам
+land = rospy.ServiceProxy('land', Trigger) #сервис для посадки
+
+
+def navigate_wait(x=0, y=0, z=1, speed=0.5, frame_id='aruco_map', auto_arm=False, tolerance=0.2,sleep=2):
+    """
+    Летит в указанную точку и ждет, пока дрон туда ДОЛЕТИТ.
+    Следующая команда выполнится ТОЛЬКО после прибытия.
+    """
+    navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
+        
+    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
+        rospy.sleep(0.2)
+    
+    rospy.sleep(sleep)
+
+
+def emergency_stop():
+
+    """Функция экстренной остановки, срабатывает при нажатии Ctrl + C"""
+ 
+    print("\n" + "="*50)
+    print("EMERGENCY STOP ACTIVATED!")
+    print("="*50)
+    
+    try:
+        print("1. Stopping movement...")
+        try:
+            navigate(x=0, y=0, z=0, frame_id='body', speed=0.5)
+            time.sleep(0.5)
+        except:
+            pass
+        
+        print("2. Hovering for safety...")
+        for i in range(3):
+            print(f"   Hovering... {3-i} seconds")
+            time.sleep(1.0)
+        
+        print("3. Emergency landing...")
+        try:
+            land()
+            time.sleep(2)
+        except:
+            try:
+                telem = get_telemetry(frame_id='aruco_map')
+                navigate(x=telem.x, y=telem.y, z=0.1, frame_id='aruco_map', speed=0.3)
+                time.sleep(5)
+            except:
+                pass
+        
+        print("✓ Emergency stop completed")
+        
+    except Exception as e:
+        print(f"Warning: {e}")
+
+
+def signal_handler(sig, frame):
+
+    """Перехват Ctrl+C: вместо аварийного завершения программы 
+    выполняет безопасную посадку дрона."""
+
+    print("\nCtrl+C detected - Emergency stop!")
+    emergency_stop()
+    sys.exit(0)
+
+
+signal.signal(signal.SIGINT, signal_handler)
+
+
+def main():
+    navigate_wait(z=1,frame_id='body',auto_arm=True) #должно быть. включает моторы и поднимает дрон на метр вверх
+    #в дальнейшем не передавать переменной auto_arm значение True
+
+    
+    <<navigate_wait>>
+ 
+
+    land() #посадка
+
+if __name__ == "__main__":
+    main()

public/shablons/qr.txt

@@ -0,0 +1,26 @@
+class QrDetect():
+    def __init__(self):
+        self.qr_data = ''
+
+    def getQrData(self):
+        return self.qr_data
+    
+    def qr_detection(self,data):
+        '''сканирование qr кода'''
+        if self.qr_data!= '':
+            return
+        cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')
+        barcodes = pyzbar.decode(cv_image)
+
+        if barcodes:
+            self.qr_data = barcodes[0].data.decode('utf-8')
+
+
+#в MAIN для вызова функции необходимо следующее:
+#1. создаём и вызываем подписчика, который вызывает функцию чтения кьюаркода
+qd = QrDetect()
+qr_sub = rospy.Subscriber('main_camera/image_raw',Image, qd.qr_detection,queue_size=1)
+#2. задаём время работы подписчика (за 1 секунду примерно 20 раз вызывается функция) 
+rospy.sleep(2) 
+#3. выключаем подписчика
+qr_sub.unregister()