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