辨識紅色

由於在空拍機影像回傳回來是用RGB的方式，cv2.IMREAD_COLOR出讀取
所以在判斷的過程中，我用了cv2.COLOR_BGR2HSV去轉換RGB成HSV的顏色數據
然後，在判斷顏色之前必須先告訴電腦紅色的顏色是怎麼樣的，而在紅色的色譜中是有兩邊，red_0和red_1去說明紅色在HSV代表的範圍
接下來才是正式的判斷
從而得到下圖

import cv2
import numpy as np

img = cv2.imread('red.jpg', cv2.IMREAD_COLOR)
hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

lower_red_0 = np.array([0, 70, 0]) 
upper_red_0 = np.array([5, 255, 255])
lower_red_1 = np.array([175, 70, 0]) 
upper_red_1 = np.array([180, 255, 255])
red_mask0 = cv2.inRange(hsv_img, lower_red_0, upper_red_0)
red_mask1 = cv2.inRange(hsv_img, lower_red_1, upper_red_1)
red_mask = cv2.bitwise_or(red_mask0, red_mask1) 

cv2.imshow("red_mask", red_mask) 
cv2.imshow("test", img)
cv2.waitKey(0)

找出紅色的輪廓

為何辨識了還不夠呢?
因為本次我是想空拍機飛越一個紅色的圈圈
那麼就必須是收最大面積的外輪廓，不隨便看到紅色就會撞過去
用cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE去找出一個圈的紅色，並填滿中空的部分，以確保是最大面積的紅色

img = cv2.imread('red.jpg')
print(img.shape)
hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# red hsv range and mask on hsv_img
red_mask = findMask(hsv_img)
print(red_mask.shape)
(contour_i, contour_contours, contour_h) = cv2.findContours(red_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
contour_i = red_mask.copy()
contour_i = cv2.cvtColor(contour_i, cv2.COLOR_GRAY2BGR)
cv2.drawContours(contour_i, contour_contours, -1, (0, 0, 255), 2)
# show red mask
cv2.imshow("red_mask", red_mask)
cv2.imshow("test", contour_i)
cv2.imwrite("red_mask.jpg", red_mask)
cv2.imwrite("test.jpg", contour_i)
cv2.waitKey(0)

找出中心點

畢竟是要穿越紅色的框，所以我們要計算紅色的中心點
用紅色的輪廓的XY軸去計算當中的mean

cv2.drawContours(contour_i, contour_contours, -1, (0, 0, 255), 2)
# show red mask
avg_x = []
avg_y = []
for cnt in contour_contours:
  for c in cnt:
    avg_x.append(c[0][0])
    avg_y.append(c[0][1])
print(np.mean(avg_x))
print(np.mean(avg_y))
cv2.circle(contour_i, (int(np.mean(avg_x)), int(np.mean(avg_y))), 1, (0,0,255), -1)
cv2.imshow("test_center", contour_i)
cv2.imwrite("test_center.jpg", contour_i)
cv2.waitKey(0)

最後我們就可以把這些判斷放到訂閱者Subscriber上

那發佈者Publisher呢??

現在來到發佈者Publisher的部份了，空拍機看到紅色也知道點了那甚麼時候才是飛越的最佳的時間呢?
在空拍機向前飛的時候會有前傾的行為，那看到的影像就會變成了紅色框框的下方，同樣地在紅色最大面積輪廓方面就會因此出現問題了。
在最大面積輪廓上會抓成下方的框邊然後直直撞上.....
所以在飛行的路線上我們要有點上調，同時因為每一次飛行的距離可能都有落差。
各位還是要自己再調。

def cmd():
  #The queue_size argument is New in ROS hydro and limits the amount of queued messages if any subscriber is not receiving them fast enough
  rospy.init_node('h264_pub', anonymous=True)
  self_pub = rospy.Subscriber('/selfDefined', test, callback)
  cmd_pub = rospy.Publisher('/tello/cmd_vel', Twist, queue_size = 10)
  rate = rospy.Rate(10)

  count = 0
  global target
  global center
  global check

  while target[0] == -1 and target[1] == -1:
    pass 

  while not rospy.is_shutdown():
    dx = target[0] - center[0]
    dy = target[1] - center[1]

    if target[2] == -1:
        msg = Twist()
        msg.linear.x = 0.2
        msg.linear.y = -dx / abs(dx) * 0.1
        msg.linear.z = -dy / abs(dy) * 0.1
        cmd_pub.publish(msg)
        rate.sleep()
        sleep(1)
        msg = Twist()
        msg.linear.x = 0.2 
        cmd_pub.publish(msg)
        rate.sleep()
        sleep(2)
        msg = Twist()
        cmd_pub.publish(msg)
        rate.sleep()
        break  
    else:
      if check == False:
        if abs(dx) < 48 and abs(dy) < 48:
          check = True
        else:
          check = False
      else:
        if abs(dx) >= 50 or abs(dy) >= 50:
          check = False

      if check == True or (dx == 0 or dy == 0):
        msg = Twist()
        msg.linear.x = 0.2
        cmd_pub.publish(msg)
        rate.sleep()
      else:
        msg = Twist()
        msg.linear.y = -dx / abs(dx) * 0.1
        msg.linear.z = -dy / abs(dy) * 0.2

        cmd_pub.publish(msg)
        rate.sleep()
        #sleep(1)  

  msg = Twist()
  cmd_pub.publish(msg)
  rate.sleep()  
  print("end loop")

接下來的下一篇將會說明FSM(Final State Machine)的控制方式