PyParrot Mambo无人机视觉飞行控制开发需求问询

Alright, let's walk through how to combine your Mambo drone flight code with OpenCV's color and line detection to build the functionality you need. Here's a clear, actionable breakdown:

We’ll integrate OpenCV’s detection logic directly into the DroneVisionGUI workflow, replacing the demo flight code with a state-based control loop:

1. Use the drone’s camera feed to detect blue lines for navigation
2. Adjust flight direction (left/right/forward) based on line angle
3. Trigger landing when red or green markers are detected
4. Leverage the existing Run button in DroneVisionGUI to start the mission

First, let’s refactor and combine your code into a cohesive system:

1. Core Detection Function #

This function processes each camera frame to detect blue lines, red markers, and green markers:

import cv2
import numpy as np
from pyparrot.Minidrone import Mambo
from pyparrot.DroneVisionGUI import DroneVisionGUI

testFlying = True  # Set to False for vision-only testing

def im_trim(img):
    # Crop frame to focus on the relevant area (adjust values if needed)
    x = 160
    y = 50
    w = 280
    h = 180
    return img[y:y + h, x:x + w]

def detect_colors_and_lines(frame):
    trimmed_frame = im_trim(frame)
    hsv_frame = cv2.cvtColor(trimmed_frame, cv2.COLOR_BGR2HSV)
    
    # Detect blue line
    lower_blue = np.array([100, 80, 100])
    upper_blue = np.array([120, 255, 255])
    blue_mask = cv2.inRange(hsv_frame, lower_blue, upper_blue)
    edges = cv2.Canny(blue_mask, 50, 150)
    lines = cv2.HoughLines(edges, 1, np.pi/180, threshold=80)
    
    line_angle = None
    if lines is not None:
        # Prioritize the line closest to the center of the frame
        min_r = float('inf')
        for r, theta in lines[:, 0]:
            angle = np.rad2deg(theta)
            # Filter for near-horizontal lines (adjust range for your use case)
            if (170 < angle < 190) or (-10 < angle < 10):
                if abs(r) < min_r:
                    min_r = abs(r)
                    line_angle = angle
    
    # Detect red marker (HSV has two red ranges)
    lower_red1 = np.array([0, 120, 70])
    upper_red1 = np.array([10, 255, 255])
    lower_red2 = np.array([170, 120, 70])
    upper_red2 = np.array([180, 255, 255])
    red_mask = cv2.bitwise_or(cv2.inRange(hsv_frame, lower_red1, upper_red1),
                              cv2.inRange(hsv_frame, lower_red2, upper_red2))
    red_detected = cv2.countNonZero(red_mask) > 5000  # Adjust threshold for marker size
    
    # Detect green marker
    lower_green = np.array([40, 40, 40])
    upper_green = np.array([70, 255, 255])
    green_mask = cv2.inRange(hsv_frame, lower_green, upper_green)
    green_detected = cv2.countNonZero(green_mask) > 5000
    
    return line_angle, red_detected, green_detected

2. Modified Flight Control Callback #

Replace the demo flight function with a loop that uses detection results to control the drone:

def demo_mambo_user_vision_function(mamboVision, args):
    mambo = args[0]
    if testFlying:
        print("Initiating takeoff...")
        mambo.safe_takeoff(5)
        
        if mambo.sensors.flying_state != "emergency":
            # Ascend to a stable height first
            mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=10, duration=1)
            mambo.smart_sleep(1)
            
            mission_active = True
            while mission_active:
                # Get the latest valid frame from the drone
                frame = mamboVision.get_latest_valid_picture()
                if frame is None:
                    mambo.smart_sleep(0.1)
                    continue
                
                # Run detection
                line_angle, red_detected, green_detected = detect_colors_and_lines(frame)
                
                # Check for landing triggers
                if red_detected or green_detected:
                    print("Landing marker detected! Initiating landing...")
                    mambo.safe_land(5)
                    mission_active = False
                    break
                
                # Line-following logic
                if line_angle is not None:
                    # Fly forward if line is horizontal
                    if (175 < line_angle < 185) or (-5 < line_angle < 5):
                        print("Flying forward along blue line")
                        mambo.fly_direct(roll=0, pitch=8, yaw=0, vertical_movement=0, duration=0.4)
                    # Adjust right if line tilts left
                    elif line_angle > 185:
                        print("Adjusting right to align with line")
                        mambo.fly_direct(roll=6, pitch=5, yaw=0, vertical_movement=0, duration=0.4)
                    # Adjust left if line tilts right
                    elif line_angle < -5:
                        print("Adjusting left to align with line")
                        mambo.fly_direct(roll=-6, pitch=5, yaw=0, vertical_movement=0, duration=0.4)
                else:
                    # Hover if no line is detected
                    print("No blue line found - hovering")
                    mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=0.4)
                
                mambo.smart_sleep(0.2)
    else:
        print("Vision-only mode active - no flight. Sleeping for 15 seconds.")
        mambo.smart_sleep(15)
    
    # Cleanup
    mamboVision.close_video()
    mambo.smart_sleep(5)
    print("Disconnecting drone")
    mambo.disconnect()

class UserVision:
    def __init__(self, vision):
        self.index = 0
        self.vision = vision

    def save_pictures(self, args):
        # Optional: Save frames for debugging
        img = self.vision.get_latest_valid_picture()
        if img is not None:
            filename = "drone_frame_%06d.png" % self.index
            # cv2.imwrite(filename, img)  # Uncomment to save frames
            self.index +=1

if __name__ == "__main__":
    mamboAddr = "B0:FC:36:F4:37:F9"  # Replace with your drone's address
    mambo = Mambo(mamboAddr, use_wifi=True)
    
    print("Connecting to Mambo...")
    success = mambo.connect(num_retries=3)
    print("Connected: %s" % success)
    
    if success:
        mambo.smart_sleep(1)
        mambo.ask_for_state_update()
        mambo.smart_sleep(1)
        
        print("Starting vision feed...")
        mamboVision = DroneVisionGUI(mambo, is_bebop=False, buffer_size=200,
                                     user_code_to_run=demo_mambo_user_vision_function,
                                     user_args=(mambo,))
        
        userVision = UserVision(mamboVision)
        mamboVision.set_user_callback_function(userVision.save_pictures, user_callback_args=None)
        
        mamboVision.open_video()

• Tune Color Thresholds: HSV values vary with lighting. Test your blue/red/green thresholds locally with a webcam first (use cv2.inRange and display masks) before deploying to the drone.
• Adjust Flight Parameters: The pitch, roll, and duration values in fly_direct depend on your drone’s sensitivity. Start with small values (5-10) to avoid aggressive movements.
• Thread Safety: Always check if frame is valid before processing it—drone camera feeds can drop frames occasionally.
• Flight State Checks: Add checks for mambo.sensors.flying_state before sending commands to avoid sending instructions to a grounded or emergency-state drone.
• Button Trigger: The Run button in the DroneVisionGUI window will start your mission automatically—no extra code needed for button activation.

内容的提问来源于stack exchange，提问作者이중현