def __setbutton_click(self, wait=False):
if not self.client.is_open():
write_log("ERROR: Not connected to client")
return
# writing message counter
retval = self.__write_register(COUNTER_REGISTER_OUT, self.counter)
if not retval:
self.__update_gui()
return
# writing registers
for address in self.register_values_widgets:
value, widget = self.register_values_widgets[address]
widgetvalue_int = None
try:
widgetvalue_int = int(widget.get())
except ValueError:
write_log(
"ERROR: Wrong input format in value entry for address: %d"
% address)
continue
if value == widgetvalue_int:
continue
retval = self.__write_register(address, widgetvalue_int)
if retval:
self.register_values_widgets[address] = (widgetvalue_int,
widget)
else:
self.__update_gui()
self.refresh_values()
# message counter wait
if wait:
global break_wait
while not break_wait:
with self.lock:
counter = self.client.read_input_registers(
COUNTER_REGISTER_IN)[0]
if counter == self.counter:
break
time.sleep(0.1)
break_wait = False
# counter increment
self.counter = (self.counter + 1) % 20
if PRINT_ALL_MEMORY_ON_WRITE:
self.__print_memory()
self.read_robot_pos()
def __connectbutton_click(self):
if self.client.is_open():
self.client.close()
else:
self.client.host(SERVER_HOST)
self.client.port(SERVER_PORT)
if self.client.open():
write_log("Connection established")
self.refresh_values()
self.read_robot_pos()
else:
write_log("ERROR: Connecting failed")
self.__update_gui()
def __connectbutton_click(self):
if self.client.is_open():
self.client.close()
else:
self.client.host(SERVER_HOST)
self.client.port(SERVER_PORT)
if self.client.open():
write_log("Connection established")
self.refresh_values()
self.read_robot_pos()
else:
write_log("ERROR: Connecting failed")
self.__update_gui()
def __print_memory(self):
self.refresh_values()
write_log("Memory dump:")
write_log("------------")
for address in self.register_values_widgets:
val, widget = self.register_values_widgets[address]
write_log("%d, %d" % (address, val))
write_log("------------")
def __print_memory(self):
self.refresh_values()
write_log("Memory dump:")
write_log("------------")
for address in self.register_values_widgets:
val, widget = self.register_values_widgets[address]
write_log("%d, %d" % (address, val))
write_log("------------")
def __setbutton_click(self, wait = False):
if not self.client.is_open():
write_log("ERROR: Not connected to client")
return
# writing message counter
retval = self.__write_register(COUNTER_REGISTER_OUT, self.counter)
if not retval:
self.__update_gui()
return
# writing registers
for address in self.register_values_widgets:
value, widget = self.register_values_widgets[address]
widgetvalue_int = None
try:
widgetvalue_int = int(widget.get())
except ValueError:
write_log("ERROR: Wrong input format in value entry for address: %d" % address)
continue
if value == widgetvalue_int:
continue
retval = self.__write_register(address, widgetvalue_int)
if retval:
self.register_values_widgets[address] = (widgetvalue_int, widget)
else:
self.__update_gui()
self.refresh_values()
# message counter wait
if wait:
global break_wait
while not break_wait:
with self.lock:
counter = self.client.read_input_registers(COUNTER_REGISTER_IN)[0]
if counter == self.counter:
break
time.sleep(0.1)
break_wait = False
# counter increment
self.counter = (self.counter + 1) % 20
if PRINT_ALL_MEMORY_ON_WRITE:
self.__print_memory()
self.read_robot_pos()
def refresh_values(self):
for address in self.register_values_widgets:
if self.client.is_open():
value, widget = self.register_values_widgets[address]
with self.lock:
real_val_uint = self.client.read_input_registers(address)[0]
real_val_holding_uint = self.client.read_holding_registers(address)[0]
assert real_val_uint == real_val_holding_uint
real_val_int = uintToInt16(real_val_uint)
widget.set(str(real_val_int))
self.register_values_widgets[address] = (real_val_int, widget)
else:
write_log("ERROR: Read could not be completed, client not connected.")
self.__update_gui()
break
write_log("Refresh done.")
return self.register_values_widgets
def refresh_values(self):
for address in self.register_values_widgets:
if self.client.is_open():
value, widget = self.register_values_widgets[address]
with self.lock:
real_val_uint = self.client.read_input_registers(
address)[0]
real_val_holding_uint = self.client.read_holding_registers(
address)[0]
assert real_val_uint == real_val_holding_uint
real_val_int = uintToInt16(real_val_uint)
widget.set(str(real_val_int))
self.register_values_widgets[address] = (real_val_int, widget)
else:
write_log(
"ERROR: Read could not be completed, client not connected."
)
self.__update_gui()
break
write_log("Refresh done.")
return self.register_values_widgets
def __write_register(self, address, value):
if not (-32768 <= value <= 32767):
write_log("ERROR: -32768 <= value <= 32767 is false for address: %d" % address)
return False
widgetvalue_uint = intToUint16(value)
if self.client.is_open():
with self.lock:
retval = self.client.write_single_register(address, widgetvalue_uint)
if retval:
write_log("Register written. Address: %d, value: %d" % (address, value))
return True
else:
write_log("ERROR: Write failed. Address: %d, value: %d" % (address, value))
else:
write_log("ERROR: client not connected.")
return False
def read_robot_pos(self):
write_log("Reading robot position:")
posdict = {}
for i in range(1000, 1006):
if self.client.is_open():
with self.lock:
real_val_uint = self.client.read_input_registers(i)[0]
real_val_holding_uint = self.client.read_holding_registers(i)[0]
assert real_val_uint == real_val_holding_uint
real_val_int = uintToInt16(real_val_uint)
posdict[i] = real_val_int
write_log("%d, %d" % (i, real_val_int))
else:
write_log("ERROR: Read could not be completed, client not connected.")
self.__update_gui()
break
write_log("Read done.")
return posdict
def read_robot_pos(self):
write_log("Reading robot position:")
posdict = {}
for i in range(1000, 1006):
if self.client.is_open():
with self.lock:
real_val_uint = self.client.read_input_registers(i)[0]
real_val_holding_uint = self.client.read_holding_registers(
i)[0]
assert real_val_uint == real_val_holding_uint
real_val_int = uintToInt16(real_val_uint)
posdict[i] = real_val_int
write_log("%d, %d" % (i, real_val_int))
else:
write_log(
"ERROR: Read could not be completed, client not connected."
)
self.__update_gui()
break
write_log("Read done.")
return posdict
def __write_register(self, address, value):
if not (-32768 <= value <= 32767):
write_log(
"ERROR: -32768 <= value <= 32767 is false for address: %d" %
address)
return False
widgetvalue_uint = intToUint16(value)
if self.client.is_open():
with self.lock:
retval = self.client.write_single_register(
address, widgetvalue_uint)
if retval:
write_log("Register written. Address: %d, value: %d" %
(address, value))
return True
else:
write_log("ERROR: Write failed. Address: %d, value: %d" %
(address, value))
else:
write_log("ERROR: client not connected.")
return False
def run(out_folder):
global capture
if out_folder is None:
out_folder = logger.outputdir
cap = cv2.VideoCapture(0)
if not cap.isOpened():
cap = cv2.VideoCapture(1)
resolution = (960, 720)
cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, resolution[0])
cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, resolution[1])
diff_frame = cv2.imread("out/2017_5_17__14_15_12/0000.jpg")
diff_frame = np.zeros_like(diff_frame, dtype="uint8")
fileIdx = getNextFileIdx(out_folder)
if not cap.isOpened():
write_log("ERROR: webcam open failed")
else:
cv2.namedWindow("frame")
if WINDOW_POS:
x, y = WINDOW_POS
cv2.moveWindow("frame", x, y)
while cap.isOpened():
if exit:
break
r, frame = cap.read()
if not r:
continue
if resolution[0] not in frame.shape or resolution[
1] not in frame.shape:
# bad resolution
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
frame = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
cv2.putText(frame, "BAD RESOLUTION: %s" % (str(gray.shape)),
(0, gray.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX,
1, (0, 0, 255), 2)
cv2.imshow("frame", frame)
key = cv2.waitKey(1)
if key != -1:
print "pressed key: %d" % key
if key == 27:
break
continue
# cv2.circle(frame, (960 / 2, 720 / 2), 10, (255, 0, 0), 4)
cv2.imshow("frame", frame)
diff = np.uint8(np.abs(np.int16(diff_frame) - np.int16(frame)))
cv2.imshow("frame", diff)
# enter: 13, escape: 27, space: 32
key = cv2.waitKey(1)
if key == 27:
break
if key == 32 or capture:
if capture: capture = False
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(
gray, (9, 6),
flags=cv2.CALIB_CB_ADAPTIVE_THRESH
| cv2.CALIB_CB_NORMALIZE_IMAGE)
if ret:
key = 13
write_log("Chessboard found")
else:
if capture_if_no_chessboard:
key = 13
write_log("Chessboard not found.")
if key == 13:
filename = getFileName(out_folder, fileIdx)
success = cv2.imwrite(filename, frame)
if success:
write_log("Success. File saved: %s" % filename)
dictfile = os.path.splitext(filename)[0] + ".p"
dumpDict(dictfile)
else:
write_log("Failed to write to: %s" % filename)
fileIdx += 1
def run(out_folder):
global capture
if out_folder is None:
out_folder = logger.outputdir
cap = cv2.VideoCapture(0)
if not cap.isOpened():
cap = cv2.VideoCapture(1)
resolution = (960, 720)
cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, resolution[0])
cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, resolution[1])
diff_frame = cv2.imread("out/2017_5_17__14_15_12/0000.jpg")
diff_frame = np.zeros_like(diff_frame, dtype="uint8")
fileIdx = getNextFileIdx(out_folder)
if not cap.isOpened():
write_log("ERROR: webcam open failed")
else:
cv2.namedWindow("frame")
if WINDOW_POS:
x, y = WINDOW_POS
cv2.moveWindow("frame", x, y)
while cap.isOpened():
if exit:
break
r, frame = cap.read()
if not r:
continue
if resolution[0] not in frame.shape or resolution[1] not in frame.shape:
# bad resolution
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
frame = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
cv2.putText(frame, "BAD RESOLUTION: %s" % (str(gray.shape)), (0, gray.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
cv2.imshow("frame", frame)
key = cv2.waitKey(1)
if key != -1:
print "pressed key: %d" % key
if key == 27:
break
continue
# cv2.circle(frame, (960 / 2, 720 / 2), 10, (255, 0, 0), 4)
cv2.imshow("frame", frame)
diff = np.uint8(np.abs(np.int16(diff_frame) - np.int16(frame)))
cv2.imshow("frame", diff)
# enter: 13, escape: 27, space: 32
key = cv2.waitKey(1)
if key == 27:
break
if key == 32 or capture:
if capture: capture = False
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (9, 6), flags=cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE)
if ret:
key = 13
write_log("Chessboard found")
else:
if capture_if_no_chessboard:
key = 13
write_log("Chessboard not found.")
if key == 13:
filename = getFileName(out_folder, fileIdx)
success = cv2.imwrite(filename, frame)
if success:
write_log("Success. File saved: %s" % filename)
dictfile = os.path.splitext(filename)[0]+".p"
dumpDict(dictfile)
else:
write_log("Failed to write to: %s" % filename)
fileIdx += 1
def __find_object(self):
import DataCache as DC
from glob import glob
from os.path import join
import numpy as np
from SFMSolver import SFMSolver, find_ext_params
import Utils
print "FINDING"
np.set_printoptions(precision=3, suppress=True)
files_dir = "out/2017_3_8__14_51_22/"
files = glob(join(files_dir, "*.jpg"))
masks = []
for f in files:
m = f.replace(".jpg", "_mask.png")
masks.append(m)
sfm = SFMSolver(files, masks)
if self.obj_data is None:
imgs, kpts, points, data = sfm.calc_data_from_files_triang_simple()
self.obj_data = imgs, kpts, points, data
else:
imgs, kpts, points, data = self.obj_data
arr_calib = DC.getData("out/%s/arrangement_calib.p" %
ARRANGEMENT_CALIB_DIR)
ttc = arr_calib["ttc"]
tor = arr_calib["tor"]
if "cam_mtx" in arr_calib:
print "camMtx, distcoeffs load"
Utils.camMtx = arr_calib["cam_mtx"]
Utils.dist_coeffs = arr_calib["dist_coeffs"]
if self.stop_signal:
self.stop_signal = False
return
for point in FIND_POINTS:
values = {
500: point[0],
501: point[1],
502: point[2],
503: point[3],
504: point[4],
505: point[5],
}
print "set_values call"
self.set_values(values, True)
print "set_values return"
time.sleep(0.5)
CamGrabber.capture_if_no_chessboard = True
CamGrabber.capture = True
time.sleep(0.5)
if self.stop_signal:
self.stop_signal = False
return
find_dir = logger.outputdir
files = glob("%s/*.jpg" % find_dir)
print files
# files_dir = "out/2017_4_5__15_57_20/"
# files = glob(join(files_dir, "*.jpg"))
files.sort()
files = files[-len(FIND_POINTS):]
results = []
for f in files:
res = find_ext_params(f, imgs, kpts, points, data, tor, ttc)
results.append(res)
if self.stop_signal:
self.stop_signal = False
return
for i in range(len(results)):
print i, results[i]
write_log((i, results[i]))
result = max(results, key=lambda x: x[2])
write_log(result)
values = {
500: int(result[0][0] * 10),
501: int(result[0][1] * 10),
502: int(result[0][2] * 10) + 200,
503: int(result[1][2]),
504: int(result[1][1]),
505: int(result[1][0]),
}
print "num inl: ", result[2]
pprint(values)
self.set_values(values, go_to_value=False)
self.find_thread = None
def __find_object(self):
import DataCache as DC
from glob import glob
from os.path import join
import numpy as np
from SFMSolver import SFMSolver, find_ext_params
import Utils
print "FINDING"
np.set_printoptions(precision=3, suppress=True)
files_dir = "out/2017_3_8__14_51_22/"
files = glob(join(files_dir, "*.jpg"))
masks = []
for f in files:
m = f.replace(".jpg", "_mask.png")
masks.append(m)
sfm = SFMSolver(files, masks)
if self.obj_data is None:
imgs, kpts, points, data = sfm.calc_data_from_files_triang_simple()
self.obj_data = imgs, kpts, points, data
else:
imgs, kpts, points, data = self.obj_data
arr_calib = DC.getData("out/%s/arrangement_calib.p" % ARRANGEMENT_CALIB_DIR)
ttc = arr_calib["ttc"]
tor = arr_calib["tor"]
if "cam_mtx" in arr_calib:
print "camMtx, distcoeffs load"
Utils.camMtx = arr_calib["cam_mtx"]
Utils.dist_coeffs = arr_calib["dist_coeffs"]
if self.stop_signal:
self.stop_signal = False
return
for point in FIND_POINTS:
values = {
500: point[0],
501: point[1],
502: point[2],
503: point[3],
504: point[4],
505: point[5],
}
print "set_values call"
self.set_values(values, True)
print "set_values return"
time.sleep(0.5)
CamGrabber.capture_if_no_chessboard = True
CamGrabber.capture = True
time.sleep(0.5)
if self.stop_signal:
self.stop_signal = False
return
find_dir = logger.outputdir
files = glob("%s/*.jpg" % find_dir)
print files
# files_dir = "out/2017_4_5__15_57_20/"
# files = glob(join(files_dir, "*.jpg"))
files.sort()
files = files[-len(FIND_POINTS):]
results = []
for f in files:
res = find_ext_params(f, imgs, kpts, points, data, tor, ttc)
results.append(res)
if self.stop_signal:
self.stop_signal = False
return
for i in range(len(results)):
print i, results[i]
write_log((i, results[i]))
result = max(results, key=lambda x: x[2])
write_log(result)
values = {
500: int(result[0][0] * 10),
501: int(result[0][1] * 10),
502: int(result[0][2] * 10) + 200,
503: int(result[1][2]),
504: int(result[1][1]),
505: int(result[1][0]),
}
print "num inl: ", result[2]
pprint(values)
self.set_values(values, go_to_value=False)
self.find_thread = None