修改此代码使其可重复运行import pygame
import sys
from pygame.locals import *
from robomaster import *
import cv2
import numpy as np
focal_length = 750 # 焦距
known_radius = 2 # 已知球的半径
def calculate_distance(focal_length, known_radius, perceived_radius):
distance = (known_radius * focal_length) / perceived_radius
return distance
def show_video(ep_robot, screen):
获取机器人第一视角图像帧
img = ep_robot.camera.read_cv2_image(strategy="newest")
转换图像格式,转换为pygame的surface对象
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
img = cv2.transpose(img) # 行列互换
img = pygame.surfarray.make_surface(img)
screen.blit(img, (0, 0)) # 绘制图像
def detect_white_circle(ep_robot):
获取机器人第一视角图像帧
img = ep_robot.camera.read_cv2_image(strategy="newest")
转换为灰度图像
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
进行中值滤波处理
gray = cv2.medianBlur(gray, 5)
检测圆形轮廓
circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 50, param1=160, param2=40, minRadius=5, maxRadius=60)
if circles is not None:
circles = np.uint16(np.around(circles))
for circle in circles[0, :]:
center = (circle[0], circle[1])
known_radius = circle
在图像上绘制圆形轮廓
cv2.circle(img, center, known_radius, (0, 255, 0), 2)
显示图像
distance = calculate_distance(focal_length, known_radius, known_radius)
在图像上绘制圆和距离
cv2.circle(img, center, known_radius, (0, 255, 0), 2)
cv2.putText(img, f"Distance: {distance:.2f} cm", (center[0] - known_radius, center[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.imshow("White Circle Detection", img)
cv2.waitKey(1)
def main():
pygame.init()
screen_size = width, height = 1280, 720
screen = pygame.display.set_mode(screen_size)
ep_robot = robot.Robot()
ep_robot.initialize(conn_type='ap')
version = ep_robot.get_version()
print("Robot version: {0}".format(version))
ep_robot.camera.start_video_stream(display=False)
pygame.time.wait(100)
clock = pygame.time.Clock()
while True:
clock.tick(5) # 将帧数设置为25帧
for event in pygame.event.get():
if event.type == QUIT:
ep_robot.close()
pygame.quit()
sys.exit()
show_video(ep_robot, screen)
detect_white_circle(ep_robot)
if name == 'main':
main()