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()
