def run(self): estimator = PoseEstimator("./usb_camera.yml") # 2. 加载图片 capture = cv2.VideoCapture(2) if not capture.isOpened(): print("摄像头打开失败!") return capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 960) try: while not rospy.is_shutdown(): _, frame = capture.read() if self.robot.isMoving(): continue rst, image, pose = estimator.estimate(frame) cv2.imshow("img", image) if rst: print(("xyz-rpy: " + "{:>8.3f}," * 6).format( pose[0], pose[1], pose[2], pose[3], pose[4], pose[5])) self.robot.move_rotation(radians(-pose[4])) else: print("未发现目标") #self.robot.move_rotation(radians(90)) self.robot.stop() action = cv2.waitKey(30) & 0xff if action == ord('s'): cv2.imwrite("save_line.jpg", image) elif action == 27: break self.rate.sleep() except KeyboardInterrupt as e: print("Ctrl + c 主动停止了程序") finally: capture.release() cv2.destroyAllWindows()
running = True while running: clock.tick(FPS) screen.fill(BLACK) for i in range(9): pygame.draw.line(screen, (0, 200, 200), (0, 50 * (i + 1)), (500, 50 * (i + 1)), 1) pygame.draw.line(screen, (0, 200, 200), (50 * (i + 1), 0), (50 * (i + 1), 500), 1) for event in pygame.event.get(): if event.type == pygame.QUIT: running = False keys = pygame.key.get_pressed() if keys[pygame.K_UP]: tri.drive(1) if keys[pygame.K_DOWN]: tri.drive(-1) if keys[pygame.K_LEFT]: tri.turn_front_wheel(np.pi / 180) if keys[pygame.K_RIGHT]: tri.turn_front_wheel(-np.pi / 180) tri.draw() print( pose_est.estimate(pygame.time.get_ticks(), tri.front_wheel_angle, tri.ticks, 0)) pygame.display.flip() pygame.quit()