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