def __init__(self, marbleStateManager, setpointManager):
self.marbleStateManager = marbleStateManager
self.controller = SimplePIDController(marbleStateManager)
self.capturer = RealsenseCapturer()
self.detector = SimpleBlobDetector.createMarbleDetector()
self.motorDriverX = MotorDriver.create_motor_x()
self.motorDriverY = MotorDriver.create_motor_y()
self.setpointManager = setpointManager
def display_simple_detection_results():
capturer = RealsenseCapturer()
detector = SimpleBlobDetector.createBlueMarbleDetector()
while(True):
# Capture frame-by-frame
frame = capturer.getCroppedFrame()
filtered_image, blob = detector.computeAverageThreshold(frame)
mark_cell(filtered_image, blob[0], blob[1], RED, 3)
cv2.imshow('frame', filtered_image)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
capturer.shutdown()
def track_marble(capturer):
detector = SimpleBlobDetector.createBlueMarbleDetectorRS()
# first couple frames are bad
for i in range(40):
frame = capturer.getCroppedFrame()
# time.sleep(0.2)
frame = capturer.getCroppedFrame()
cv2.imshow('image', frame)
cv2.setMouseCallback('image', WebcamCapturer.mouse_callback)
while(True):
if (WebcamCapturer.has_clicked or (cv2.waitKey(1) & 0xFF == ord('q'))):
break
marbleStateManager = MarbleStateManager()
marbleStateManager.initialize(float(WebcamCapturer.mouse_click_x), float(WebcamCapturer.mouse_click_y))
detectedMarbleLastTick = True
while(True):
frame = capturer.getCroppedFrame()
if (detectedMarbleLastTick):
filtered_image, blob = detector.computeMarblePosition(frame, marbleStateManager)
else:
filtered_image, blob = detector.computeAverageThreshold(frame)
if (blob[0] != -1):
marbleStateManager.new_state_detected(blob[0], blob[1])
mark_cell(filtered_image, marbleStateManager.x, marbleStateManager.y, RED, 3)
detectedMarbleLastTick = True
print(str(marbleStateManager.x) + ', ' + str(marbleStateManager.y))
else:
detectedMarbleLastTick = False
print("No marble detected")
cv2.imshow('frame', filtered_image)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
capturer.shutdown()
def go_to_setpoint(capturer):
setpointX = 200
setpointY = 200
detector = SimpleBlobDetector.createBlueMarbleDetectorRS()
motorDriverX = MotorDriver.create_motor_x()
motorDriverY = MotorDriver.create_motor_y()
motorDriverX.enable_torque()
motorDriverY.enable_torque()
motorDriverX.go_to_level()
motorDriverY.go_to_level()
# first couple frames are bad
for i in range(3):
frame = capturer.getCroppedFrame()
time.sleep(0.2)
frame = capturer.getCroppedFrame()
time.sleep(0.2)
cv2.imshow('image', frame)
cv2.setMouseCallback('image', WebcamCapturer.mouse_callback)
while(True):
if (WebcamCapturer.has_clicked or (cv2.waitKey(1) & 0xFF == ord('q'))):
break
marbleStateManager = MarbleStateManager()
marbleStateManager.initialize(float(WebcamCapturer.mouse_click_x), float(WebcamCapturer.mouse_click_y))
detectedMarbleLastTick = True
while(True):
frame = capturer.getCroppedFrame()
if (detectedMarbleLastTick):
filtered_image, blob = detector.computeMarblePosition(frame, marbleStateManager)
else:
filtered_image, blob = detector.computeAverageThreshold(frame)
if (blob[0] != -1):
if (detectedMarbleLastTick):
marbleStateManager.new_state_detected(blob[0], blob[1])
else:
marbleStateManager.initialize(blob[0], blob[1])
mark_cell(filtered_image, marbleStateManager.x, marbleStateManager.y, RED, 6)
mark_cell(filtered_image, setpointX, setpointY, GREEN, 6)
detectedMarbleLastTick = True
else:
detectedMarbleLastTick = False
dx = setpointX - marbleStateManager.x
dy = setpointY - marbleStateManager.y
vx = marbleStateManager.vx
vy = marbleStateManager.vy
kp = 0.7
kd = 0.5
signX = 1.0
signY = -1.0
ux = signX * (kp * dx - kd * vx)
uy = signY * (kp * dy - kd * vy)
if (abs(vx) < 10 and abs(vy) < 10 and abs(dx) > 20 and abs(dy) > 20):
ux = ux + signX * signum(dx) * 20
uy = uy + signY * signum(dy) * 20
ux = clamp(ux, 70)
uy = clamp(uy, 70)
motorDriverX.set_goal_relative_to_level(int(ux))
motorDriverY.set_goal_relative_to_level(int(uy))
print(str(dx) + ', ' + str(dy) + ', ' + str(marbleStateManager.vx) + ', ' + str(marbleStateManager.vy))
cv2.imshow('frame', filtered_image)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
time.sleep(1.0)
motorDriverX.disable_torque()
motorDriverY.disable_torque()
time.sleep(0.1)
motorDriverX.shutdown()
motorDriverY.shutdown()
capturer.shutdown()
mark_cell(filtered_image, start[0], start[1], GREEN, 3)
mark_cell(filtered_image, goal[0], goal[1], RED, 3)
dirname = os.path.dirname(__file__)
filename = os.path.join(dirname, '..\\..\\..\\resources\\planner_result.png')
cv2.imwrite(filename, filtered_image)
cv2.imshow('image', filtered_image)
cv2.waitKey(0)
if __name__ == "__main__":
# capturer = WebcamCapturer()
capturer = RealsenseCapturer()
# detector = SimpleBlobDetector.createMarbleDetector()
detector = SimpleBlobDetector.createObstacleDetector()
# show cropped, unfiltered image
# display_camera(capturer)
# write current webcam image to file
image_name = "Tmp.png"
# save_current_image(image_name, capturer)
# save N images
# save_a_few_images(capturer, 'Test', 20)
# run detector on saved image
# test_saved_images(detector, "Test", 3, 20)
# run detector on specific image
def __init__(self, marbleStateManager):
self.marbleStateManager = marbleStateManager
self.capturer = RealsenseCapturer()
self.marble_detector = SimpleBlobDetector.createMarbleDetector()
self.initialize = True
self.first_tick = True
def __init__(self, marbleStateManager):
self.marbleStateManager = marbleStateManager
self.obstacle_detector = SimpleBlobDetector.createObstacleDetector()
self.planner = PathPlanner()