コード例 #1
0
ファイル: test_vectors.py プロジェクト: purduerov/X12-Core
def test_get_thrust_vect(line_fail):
    v = Vector()

    Vector.prev_vector = [2, 1]
    st_pt = v.get_vector_start_point()
    center = [Vector.x_cam_width / 2 + randint(-80, 80), Vector.y_cam_height / 2 + randint(-80, 80)]
    v.get_thrust_vect(st_pt, center)
    if not np.array_equal(np.add(Vector.prev_vector, v.thrust_vect), v.resultant_vect) and line_fail[0] == -1:
        line_fail[0] = 36
        print("prev_vector:", Vector.prev_vector)
        print("thrust_vector:", v.thrust_vect)
        print("resultant_vector:", v.resultant_vect)
コード例 #2
0
def process(data):
    # convert img to cv image and convert to HSV
    img = bridge.imgmsg_to_cv2(data, "bgr8")
    img_og = img

    img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # blurring (median and then gaussian)
    img = cv2.medianBlur(img, 5)
    img = cv2.GaussianBlur(img, (5, 5), 0)

    # red
    # img = cv2.inRange(img,thresh_rngs["blue"][0],thresh_rngs["blue"][1])
    # blue
    img = cv2.inRange(img, (182 / 2, 20 * 2.56, 20 * 2.56),
                      (225 / 2, 100 * 2.56, 100 * 2.56))

    # erode and dilate image
    img = cv2.erode(img, np.ones((5, 5)))
    img = cv2.dilate(img, np.ones((10, 10)))

    # contouring
    contour = get_largest(img)
    view = View(contour)
    vects = Vector()

    if View.at_beginning:
        print("at beginning")
        '''
        #Code to be run only at the start
        sq_cnts, circ_cnts = get_ex_cnts()
        wall_md_pt, center, init_shape = match_beginning(img,view.cnt,sq_cnts,circ_cnts)
    
        Vector.prev_vector = [center[0] - wall_md_pt[0],center[1] - wall_md_pt[1]]
        '''
        Vector.prev_vector = [1, 1]
        wall_md_pt = [Vector.x_cam_width / 2, Vector.y_cam_height]
        center = [Vector.x_cam_width / 2, Vector.y_cam_height / 2]

        start_point_vector = [
            center[0] - wall_md_pt[0], center[1] - wall_md_pt[1]
        ]
        curr_thrust_vect, resultant_vect = vects.get_thrust_vect(
            wall_md_pt, center)

        # Output thrust vect as cv2 line
        cv2.circle(img_og, (start_point_vector[0], start_point_vector[1]), 3,
                   (0, 0, 255), -1)
        cv2.line(img_og, (start_point_vector[0], start_point_vector[1]),
                 (wall_md_pt[0], wall_md_pt[1]), (150, 255, 255), 1)
        View.at_beginning = False
        print("Waiting")
        time.sleep(7)
        print("Done waiting")
    else:
        curr_thrust_vect, resultant_vect = traverse_line(
            img_og, view.cnt, vects)
        print("curr_thr: [%d, %d], resultant: [%d, %d]" %
              (curr_thrust_vect[0], curr_thrust_vect[1], resultant_vect[0],
               resultant_vect[1]))

    # Set resultant vect to prev_vector
    Vector.prev_vector = resultant_vect

    # show images
    cv2.imshow("Image", img_og)
    cv2.imshow("Filtered", img)
    cv2.waitKey(3)