G2A Many GEOs
SHARE
TWEET

Untitled

a guest Apr 10th, 2020 128 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. # coding: utf8
  2. import cv2
  3. import rospy
  4. import numpy as np
  5. from clever import srv
  6. from pyzbar import pyzbar
  7. from cv_bridge import CvBridge
  8. from sensor_msgs.msg import Image
  9. from clever.srv import SetLEDEffect
  10. from std_srvs.srv import Trigger
  11.  
  12. bridge = CvBridge()
  13.  
  14. lower_green = np.array([50, 50, 70])
  15. upper_green = np.array([103, 255, 255])
  16.  
  17. lower_red = np.array([10, 50, 50])
  18. upper_red = np.array([179, 255, 255])
  19.  
  20. lower_yellow = np.array([22, 93, 0])
  21. upper_yellow = np.array([45, 255, 255])
  22.  
  23.  
  24. def findcol(lower_color, upper_color, color_name):
  25.     # Take each frame
  26.     cap = rospy.wait_for_message('main_camera/image_raw', Image)
  27.     img = bridge.imgmsg_to_cv2(cap, 'bgr8')
  28.  
  29.     # Convert BGR to HSV
  30.     hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
  31.  
  32.     mask = cv2.inRange(hsv, lower_color, upper_color)
  33.     res = cv2.bitwise_and(hsv, hsv, mask=mask)
  34.  
  35.     result = ""
  36.  
  37.     if len(res) > 0:
  38.         result = color_name
  39.  
  40.     return result
  41.  
  42.  
  43. def allcolor():
  44.     if findcol(lower_green, upper_green, '-') == '-':
  45.         people.append('-')
  46.     elif findcol(lower_red, upper_red, '+') == '+':
  47.         people.append("+")
  48.         set_effect(r=128, g=0, b=128)  # пометка зараженного
  49.         print("Тест сброшен")
  50.         rospy.sleep(5)
  51.     elif findcol(lower_yellow, upper_yellow, '?') == '?':
  52.         people.append('?')
  53.         set_effect(r=128, g=0, b=128)  # пометка зараженного
  54.         print("Тест сброшен")
  55.         rospy.sleep(5)
  56.  
  57.  
  58. rospy.init_node('flight')
  59.  
  60. get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
  61. navigate = rospy.ServiceProxy('navigate', srv.Navigate)
  62. navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
  63. set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
  64. set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
  65. set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
  66. set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
  67. land = rospy.ServiceProxy('land', Trigger)
  68. cap = rospy.wait_for_message('main_camera/image_raw', Image)
  69.  
  70. people = []
  71. result = ""
  72. ans = ""
  73.  
  74. # Первый взлет
  75. # Взлет
  76. navigate(x=0, y=0, z=0.6, frame_id='body', speed=0.5, auto_arm=True)
  77.  
  78. rospy.sleep(10)
  79.  
  80. # 1
  81. navigate(x=0.295, y=0.295, z=0.6, speed=0.8, frame_id='aruco_map')
  82. allcolor()
  83. rospy.sleep(5)
  84.  
  85. # 2
  86. navigate(x=0.295, y=0.885, z=0.6, speed=0.8, frame_id='aruco_map')
  87. allcolor()
  88. rospy.sleep(5)
  89.  
  90. # 3
  91. navigate(x=0.295, y=1.475, z=0.6, speed=0.8, frame_id='aruco_map')
  92. allcolor()
  93. rospy.sleep(5)
  94.  
  95. # 4
  96. navigate(x=0.295, y=2.065, z=0.6, speed=0.8, frame_id='aruco_map')
  97. allcolor()
  98. rospy.sleep(5)
  99.  
  100. # 5
  101. navigate(x=0.59, y=2.655, z=0.6, speed=0.8, frame_id='aruco_map')
  102. allcolor()
  103. rospy.sleep(5)
  104.  
  105. # 6
  106. navigate(x=0.885, y=2.065, z=0.6, speed=0.8, frame_id='aruco_map')
  107. allcolor()
  108. rospy.sleep(5)
  109.  
  110. # 7
  111. navigate(x=0.885, y=1.475, z=0.6, speed=0.8, frame_id='aruco_map')
  112. allcolor()
  113. rospy.sleep(5)
  114.  
  115. # 8
  116. navigate(x=0.885, y=0.885, z=0.6, speed=0.8, frame_id='aruco_map')
  117. allcolor()
  118. rospy.sleep(5)
  119.  
  120. # 9
  121. navigate(x=0.885, y=0.295, z=0.6, speed=0.8, frame_id='aruco_map')
  122. allcolor()
  123. rospy.sleep(5)
  124.  
  125. # возвращение домой
  126. navigate(x=0, y=0, z=0, speed=0.8, frame_id='aruco_map')
  127. rospy.sleep(5)
  128.  
  129. # Посадка
  130. land()
  131. print(people)
  132.  
  133. rospy.sleep(120)  # ожидание на базе
  134.  
  135. bad_num = []
  136. bad_value = []
  137.  
  138. for x in range(len(people)):
  139.     if people[x] == "?" or people[x] == "+":
  140.         bad_num.append(x)
  141.  
  142. bridge = CvBridge()
  143.  
  144.  
  145. # Image subscriber callback function
  146. def image_callback(data):
  147.     cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
  148.     barcodes = pyzbar.decode(cv_image)
  149.     for barcode in barcodes:
  150.         b_data = barcode.data.encode("utf-8")
  151.         b_type = barcode.type
  152.         (x, y, w, h) = barcode.rect
  153.         xc = x + w / 2
  154.         yc = y + h / 2
  155.         cv2.putText(cv_image, "QR-код!", (xc, yc), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
  156.         print("Был найден {} со значением {} с центром в точках x={}, y={}".format(b_type, b_data, xc, yc))
  157.         if b_data == "COVID - 2019":
  158.             set_effect(r=255, g=0, b=0)  # пометка зараженного
  159.             print("Заражен")
  160.             rospy.sleep(5)
  161.         bad_value.append(b_data)
  162.  
  163.  
  164. image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
  165.  
  166. rospy.spin()
  167.  
  168.  
  169. # Функция полета по конкретным пациентам
  170. def target(num):
  171.     if num == 1:
  172.         navigate(x=0.295, y=0.295, z=0.6, speed=0.8, frame_id='aruco_map')
  173.         rospy.sleep(5)
  174.     elif num == 2:
  175.         navigate(x=0.295, y=0.885, z=0.6, speed=0.8, frame_id='aruco_map')
  176.         rospy.sleep(5)
  177.     elif num == 3:
  178.         navigate(x=0.295, y=1.475, z=0.6, speed=0.8, frame_id='aruco_map')
  179.         rospy.sleep(5)
  180.     elif num == 4:
  181.         navigate(x=0.295, y=2.065, z=0.6, speed=0.8, frame_id='aruco_map')
  182.         rospy.sleep(5)
  183.     elif num == 5:
  184.         navigate(x=0.59, y=2.655, z=0.6, speed=0.8, frame_id='aruco_map')
  185.         rospy.sleep(5)
  186.     elif num == 6:
  187.         navigate(x=0.885, y=2.065, z=0.6, speed=0.8, frame_id='aruco_map')
  188.         rospy.sleep(5)
  189.     elif num == 7:
  190.         navigate(x=0.885, y=1.475, z=0.6, speed=0.8, frame_id='aruco_map')
  191.         rospy.sleep(5)
  192.     elif num == 8:
  193.         navigate(x=0.885, y=0.885, z=0.6, speed=0.8, frame_id='aruco_map')
  194.         rospy.sleep(5)
  195.     else:
  196.         navigate(x=0.885, y=0.295, z=0.6, speed=0.8, frame_id='aruco_map')
  197.         rospy.sleep(5)
  198.  
  199.  
  200. # Второй взлет
  201. # Взлет
  202. navigate(x=0, y=0, z=0.6, frame_id='body', speed=0.5, auto_arm=True)
  203.  
  204. # Ожидание 10 секунд
  205. rospy.sleep(10)
  206.  
  207. for n in bad_num:
  208.     target(n)
  209.  
  210. # возвращение домой
  211. navigate(x=0, y=0, z=0, speed=0.8, frame_id='aruco_map')
  212.  
  213. # Проверка на правильность всех данных
  214. print(bad_num)
  215. print(bad_value)
RAW Paste Data
Ledger Nano X - The secure hardware wallet
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top