while True:
if not lidar_data.empty():
print("Scan-time:", time.time() - start_time)
start_time = time.time()
i, data = lidar_data.get()
pre_data = lf.prepare_data(data, position)
print("Datasize:", pre_data.shape)
rotation_return = Rotation(position,
size).main(pre_data, True,
last_rotation,
rotation_counter, True)
c_image, last_rotation, rotation_counter, image = rotation_return
if last_image is not None:
print(Motion(last_image, image, 10).movement2())
mainMap = lf.draw_and_add_main_map(mainMap, pre_data, position,
rotation_counter, size, 0.90, i)
new_scan_event.set()
c_image = np.hstack(
(c_image, cv2.cvtColor(mainMap, cv2.COLOR_GRAY2BGR)))
last_image = image
cv2.imshow("Karte", cv2.resize(c_image, (1000, 500)))
key = cv2.waitKey(1)
if key == ord('q'):
print("END")
cv2.destroyAllWindows()
break
if key == ord('s'):
cv2.imwrite("image{i}.jpg".format(i=i), image)
finally:
lidar.stop()
new_scan_event = Event()
get_lidar_data = Process(target=lidar.get_scan_v3, args=(new_scan_event, 3))
start_time = time.time()
last_line_map = None
grad_counter = 0
new_scan_event.set()
try:
get_lidar_data.start()
while True:
if not lidar_data.empty():
st = time.time()
i, data = lidar_data.get()
pre_data = lf.prepare_data(data, position)
mainMap = lf.draw_and_add_main_map(mainMap, pre_data, position, grad_counter, size, 0.6, i)
lineMap = lf.draw_line_map(np.zeros(size, np.uint8), pre_data)
if last_line_map is not None:
num_matches, grad, img2 = cal_rot_move(last_line_map, lineMap, 50)
new_scan_event.set()
print("Matches:", num_matches, "Grad", grad)
if num_matches < 20 or grad > 5:
new_scan_event.set()
continue
else:
grad_counter += grad
else:
img2 = np.zeros((5000, 15000))
new_scan_event.set()
image = cv2.resize(np.hstack((mainMap, lineMap, np.zeros(size))), (1500, 500))
image = np.vstack((image, cv2.resize(img2, (1500, 500))))