fix：新增例程

基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.py

@@ -0,0 +1,199 @@
+"""
+视觉跟随圆移动控制飞机飞行。
+圆形案板移动有前后，左右，上下移动
+
+"""
+import cv2
+import threading
+import time
+import VisionCaptureApi
+import PX4MavCtrlV4 as PX4MavCtrl
+import sys
+import numpy as np
+import keyboard
+import UE4CtrlAPI
+ue = UE4CtrlAPI.UE4CtrlAPI()
+
+
+"""
+初始化飞机，起飞,创建圆形目标
+"""
+vis = VisionCaptureApi.VisionCaptureApi()
+# ue.sendUE4Cmd(b"r.setres 720x405w", 0)
+ue.sendUE4Cmd(b"t.MaxFPS 30", 0)
+ue.sendUE4PosNew(3, vehicleType=809, PosE=[2, 0, -2])
+vis.jsonLoad()
+time.sleep(1)
+ue.sendUE4ExtAct(3, ActExt=[1, 0.8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
+isSuss = vis.sendReqToUE4()  # 向RflySim3D发送取图请求，并验证
+if not isSuss:  # 如果请求取图失败，则退出
+    sys.exit(0)
+vis.startImgCap()  # 开启取图，并启用共享内存图像转发，转发到填写的目录
+time.sleep(2)
+
+mav = PX4MavCtrl.PX4MavCtrler()
+mav.InitMavLoop()
+time.sleep(2)
+mav.initOffboard()
+time.sleep(2)
+mav.SendMavArm(True)
+mav.SendPosNED(0, 0, -2, 0)
+time.sleep(10)
+detect_flag = False
+
+"""
+开辟线线程,控制圆形目标移动
+"""
+
+
+Circle_pose = [2, 0, -2, 0]
+
+
+def MoveCircle(val):
+    """
+    圆板目标移动没有旋转，只有平移，且始终朝向飞机
+    """
+    global vis
+    global detect_flag
+    global CalCirle_pose
+    offset = 0.01  # 每次移动的距离
+    yaw_offset = np.deg2rad(1)
+    drone_pose = ue.getUE4Pos()
+    # if not detect_flag:
+    #     time.sleep(0.001)
+    #     continue
+    if val == "right":  # 如果按键按下，这个值为1
+        # 向右移动
+        Circle_pose[1] += offset
+    if val == "left":
+        # 向左移动
+        Circle_pose[1] -= offset
+    if val == "up":
+        # 向上移动
+        Circle_pose[2] -= offset
+    if val == "down":
+        # 向下移动
+        Circle_pose[2] += offset
+    if val == "front":
+        # 向前移动
+        Circle_pose[0] += offset
+    if val == "back":
+        # 向后移动
+        Circle_pose[0] -= offset
+    ue.sendUE4PosNew(
+        3,
+        vehicleType=809,
+        PosE=[Circle_pose[0], Circle_pose[1], Circle_pose[2]],
+        AngEuler=[0, 0, Circle_pose[3]],
+    )
+
+
+# move_circle_th = threading.Thread(target=MoveCircle)
+# move_circle_th.start()
+
+"""
+开辟线程,获取图像,并控制飞机移动
+"""
+
+
+def CalCirle(img: np.array) -> tuple():
+    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+    low_hsv = np.array([0, 43, 46])
+    high_hsv = np.array([10, 255, 255])
+    mask1 = cv2.inRange(hsv, lowerb=low_hsv, upperb=high_hsv)
+
+    lower2 = np.array([156, 43, 46])
+    upper2 = np.array([180, 255, 255])
+    mask2 = cv2.inRange(hsv, lower2, upper2)
+    mask = mask1 + mask2
+    nonzero = np.nonzero(mask)
+    if np.size(nonzero) == 0:
+        return [-1, -1], [-1, -1], [-1, -1]
+    row_min = np.min(nonzero[0])
+    row_max = np.max(nonzero[0])
+    col_min = np.min(nonzero[1])
+    col_max = np.max(nonzero[1])
+    mid_row = int((row_max + row_min) / 2)
+    mid_col = int((col_max + col_min) / 2)
+
+    cv2.line(
+        img,
+        (col_min, mid_row),
+        (col_max, mid_row),
+        (255, 255, 255),
+        1,
+    )
+    cv2.line(
+        img,
+        (mid_col, row_min),
+        (mid_col, row_max),
+        (255, 255, 255),
+        1,
+    )
+    cv2.imshow("img ", img)
+    cv2.waitKey(1)
+    # cv2.imshow("test", mask1 + mask2)
+    # cv2.waitKey(1)
+
+    return [col_min, col_max], [row_min, row_max], [mid_col, mid_row]
+
+
+def CtrlDrone():
+    global vis
+    global mav
+    global detect_flag
+    img = None
+    init_dist_x = 0
+    init_dist_y = 0
+    img_cnt_x = 320
+    img_cnt_y = 240
+    tolerance_cnt = 15
+    tolerance_y = 15
+    tolerance_x = 20
+    kx = 0.015
+    ky = 0.009
+    kz = 0.005
+    while True:
+        if vis.hasData[0]:
+            img = vis.Img[0]
+            vis.hasData[0] = False
+            # cv2.imshow("img", vis.Img[0])
+            cv2.waitKey(1)
+            [x_min, x_max], [y_min, y_max], [cnt_x, cnt_y] = CalCirle(img)
+            if x_min == -1:
+                time.sleep(0.001)
+                continue
+            if not detect_flag:
+                detect_flag = True
+            dist_x = x_max - x_min
+            dist_y = y_max - y_min
+            if init_dist_x == 0:
+                init_dist_x = dist_x
+            if init_dist_y == 0:
+                init_dist_y = dist_y
+            vx = 0
+            vy = 0
+            vz = 0
+            yaw_rate = 0
+            if abs(cnt_x - img_cnt_x) > tolerance_cnt:
+                vy = ky * (cnt_x - img_cnt_x)
+            if abs(cnt_y - img_cnt_y) > tolerance_cnt:
+                vz = kz * (cnt_y - img_cnt_y)
+            if abs(dist_y - init_dist_y) > tolerance_y:
+                vx = kx * (init_dist_y - dist_y)
+
+            mav.SendVelFRD(vx, vy, vz, 0)
+            time.sleep(0.001)
+
+
+ctrl_drone_th = threading.Thread(target=CtrlDrone)
+ctrl_drone_th.start()
+
+keyboard.add_hotkey("ctrl+up", MoveCircle, args=("front",))
+keyboard.add_hotkey("ctrl+down", MoveCircle, args=("back",))
+keyboard.add_hotkey("up", MoveCircle, args=("up",))
+keyboard.add_hotkey("down", MoveCircle, args=("down",))
+keyboard.add_hotkey("right", MoveCircle, args=("right",))
+keyboard.add_hotkey("left", MoveCircle, args=("left",))
+
+keyboard.wait()