diff --git a/shablons/color.py b/shablons/color.py new file mode 100644 index 0000000..7a7168e --- /dev/null +++ b/shablons/color.py @@ -0,0 +1,55 @@ +color='' + +def detect_color_callback(data, print_type='str'): + '''определение цвета''' + + global color + 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])) + 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: + color = 'blue' + elif g > b and g > r and g > 100: + color = 'green' + elif r > b and r > g and r > 100: + color = 'red' + else: + color = 'unknown' + return(color) + + + + + +#в MAIN для вызова функции должно быть: + + #color_sub = rospy.Subscriber('main_camera/image_raw', Image, + #lambda img: detect_color_callback(img, print_type='rgb'), + #queue_size=1) + #rospy.sleep(2) + #color_sub.unregister() + diff --git a/shablons/emergency_and_navigate.py b/shablons/emergency_and_navigate.py new file mode 100644 index 0000000..6bea297 --- /dev/null +++ b/shablons/emergency_and_navigate.py @@ -0,0 +1,109 @@ +#!/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 и подписчики + # navigate_wait(x=3,z=1,sleep=1) + # navigate_wait(y=5,x=2,z=1,sleep = 4) + + + land() #посадка + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/shablons/qr.py b/shablons/qr.py new file mode 100644 index 0000000..8cd6251 --- /dev/null +++ b/shablons/qr.py @@ -0,0 +1,26 @@ +qr_data='' + +def qr_detection(data): + '''сканирование qr кода''' + global qr_data + if qr_data!= '': + return + cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') + barcodes = pyzbar.decode(cv_image) + + if barcodes: + qr_data = barcodes[0].data.decode('utf-8') + + return qr_data + + + + +#в MAIN для вызова функции необходимо следующее: +#1. создаём и вызываем подписчика, который вызывает функцию чтения кьюаркода +qr_sub = rospy.Subscriber('main_camera/image_raw',Image, qr_detection,queue_size=1) +#2. задаём время работы подписчика (за 1 секунду примерно 20 раз вызывается функция) +rospy.sleep(2) +#3. выключаем подписчика +qr_sub.unregister() + diff --git a/shablons/readme b/shablons/readme new file mode 100644 index 0000000..e69de29