def main():
# display detected goal and distance from it
# in a live window
parser = setup_options_parser()
args = parser.parse_args()
replaying = args.replay_dir is not None
recording = args.record
pclviewer = None
if replaying:
replayer = Replayer(args.replay_dir)
mode = "stopped"
top = 120
left = 160
cv2.namedWindow("View")
cv2.createTrackbar("mode", "View", 0, 7, lambda *args: None)
cv2.createTrackbar("area_threshold", "View", 10, 500,
lambda *args: None)
cv2.createTrackbar("frame", "View", 0, len(replayer.frame_names), lambda *args: None)
with Vision(use_sensor=False) as vision:
while True:
vision.mode = cv2.getTrackbarPos("mode", "View")
vision.area_threshold = cv2.getTrackbarPos("area_threshold", "View")
_frame_i = cv2.getTrackbarPos("frame", "View")
if 0 <= _frame_i < len(replayer.frame_names):
frame_i = _frame_i
vision.get_recorded_depths(replayer, frame_i)
vision.idepth_stats()
vision.set_display()
if mode == "stopped" and vision.mode == 4:
cv2.rectangle(
vision.display, (left, top), (left + 10, top+10),
(255, 0, 0))
if mode != "stopped" and pclviewer is not None:
pclviewer.updateCloud(vision.xyz)
cv2.imshow("View", vision.display)
wait_delay = 50
if mode == "fw" and frame_i < len(replayer.frame_names) - 1:
cv2.setTrackbarPos("frame", "View", frame_i+1)
wait_delay = replayer.offset_milis(frame_i)
elif mode == "bw" and 0 < frame_i:
cv2.setTrackbarPos("frame", "View", frame_i-1)
wait_delay = replayer.offset_milis(frame_i-1)
x = cv2.waitKey(wait_delay)
if x % 128 == 27:
break
elif ord('0') <= x <= ord('7'):
cv2.setTrackbarPos("mode", "View", x - ord('0'))
elif ord('`') == x:
cv2.setTrackbarPos("mode", "View", 0)
elif ord('s') == x:
mode = "stopped"
elif ord('f') == x:
mode = 'fw'
elif ord('b') == x:
mode = 'bw'
elif ord('p') == x:
print(replayer.file_name(frame_i))
elif ord('i') == x:
cv2.imwrite("plop.jpg", vision.display);
elif ord('z') == x and libpclproc is not None:
if pclviewer is None:
pclviewer = libpclproc.process(vision.xyz)
else:
pclviewer.close()
pclviewer = None
if pclviewer is not None and not pclviewer.wasStopped():
pclviewer.spin()
if mode == "stopped" and vision.mode == 4:
if x == 65361:
# left arrow key
left -= 2
elif x == 65362:
# up arrow key
top -= 2
elif x == 65363:
# right arrow key
left += 2
elif x == 65364:
# down arrow key
top += 2
elif x == ord('p'):
print('x: ', vision.xyz[0, top:top+10, left:left+10])
print('y: ', vision.xyz[1, top:top+10, left:left+10])
print('z: ', vision.xyz[2, top:top+10, left:left+10])
cv2.destroyWindow("View")
else:
logger = DataLogger("logs")
if recording:
logger.begin_logging()
cv2.namedWindow("View")
cv2.createTrackbar("mode", "View", 0, 7, lambda *args: None)
'''
cv2.createTrackbar("area_threshold", "View", 10, 500,
lambda *args: None)
'''
cv2.createTrackbar("angle", "View", 0, 90,
lambda *args: None)
cv2.createTrackbar("velocity", "View", 1000, 10000,
lambda *args: None)
with Vision() as vision:
while True:
vision.mode = cv2.getTrackbarPos("mode", "View")
#vision.area_threshold = cv2.getTrackbarPos("area_threshold", "View")
vision.angle = cv2.getTrackbarPos("angle", "View")
vision.get_depths()
vision.idepth_stats()
vision.set_display()
logger.log_data(vision.depth, vision.ir)
cv2.imshow("View", vision.display)
x = cv2.waitKey(50)
if x % 128 == 27:
break
elif ord('0') <= x <= ord('7'):
cv2.setTrackbarPos("mode", "View", x - ord('0'))
elif ord('`') == x:
cv2.setTrackbarPos("mode", "View", 0)
cv2.destroyWindow("View")
