def test_add(self):
        prio_set_list = event._PrioritizedSetList()
        objs = [(i,) for i in range(5)]

        prio_set_list.add(0, objs[0])
        assert prio_set_list.list == [(0, {objs[0]})]

        prio_set_list.add(0, objs[1])
        assert prio_set_list.list == [(0, {objs[0], objs[1]})]

        prio_set_list.add(10, objs[2])
        assert prio_set_list.list == [(10, {objs[2]}),
                                      (0,  {objs[0], objs[1]})]

        prio_set_list.add(-10, objs[3])
        assert prio_set_list.list == [( 10, {objs[2]}),           # noqa: E201
                                      (  0, {objs[0], objs[1]}),  # noqa: E201
                                      (-10, {objs[3]})]
        prio_set_list.add(-1, objs[4])
        assert prio_set_list.list == [( 10, {objs[2]}),           # noqa: E201
                                      (  0, {objs[0], objs[1]}),  # noqa: E201
                                      ( -1, {objs[4]}),           # noqa: E201
                                      (-10, {objs[3]})]
def main():
    camera = cv2.VideoCapture(0)
    img = get_image(camera)
    while True:
        cv2.imshow('calibration', img)
        cv2.waitKey(10)
        img = get_image(camera)
        gray = make_grayscale(img)
        corners, ids, rejected = cv2.aruco.detectMarkers(gray, aruco_dict,
                                                         corners, ids)
        cv2.aruco.drawDetectedMarkers(img, corners, ids)
        if ids is not None and corners is not None \
                and len(ids) > 0 and len(ids) == len(corners):
            diamond_corners, diamond_ids = \
                cv2.aruco.detectCharucoDiamond(img, corners, ids,
                                               0.05 / 0.03, cameraMatrix=mtx,
                                               distCoeffs=dist)
            cv2.aruco.drawDetectedDiamonds(img, diamond_corners, diamond_ids)
            '''if diamond_ids is not None and len(diamond_ids) >= 4:
                break'''
            board = cv2.aruco.CharucoBoard_create(9, 6, 0.05, 0.03,
                                                  aruco_dict)
            if diamond_corners is not None and diamond_ids is not None \
                    and len(diamond_corners) == len(diamond_ids):
                count, char_corners, char_ids = \
                    cv2.aruco.interpolateCornersCharuco(diamond_corners,
                                                        diamond_ids, gray,
                                                        board)
                if count >= 3:
                    draw_axis(img, char_corners, char_ids, board)