def __init__(self): # ROS Topics: rospy.Subscriber('IGTL_STRING_IN', igtlstring, self.callbackString) rospy.Subscriber('IGTL_TRANSFORM_IN', igtltransform, self.callbackTransformation) self.pub1 = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) self.pub2 = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) self.motors = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) self.galilStatus = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) rospy.init_node('talker', anonymous=True) # Define the variables for openigtlink self.TransferData1 = igtlstring() self.TransferData1.name = "statusTarget" self.TransferData2 = igtlstring() self.TransferData2.name = "statusZ-Frame" self.motorsData = igtlstring() self.motorsData.name = "motorPosition" self.galilStatusData = igtlstring() self.galilStatusData.name = "status" #Variables: self.status = 0 self.CartesianPositionA = 0 self.CartesianPositionB = 0 self.OrientationA = 0 self.OrientationB = 0 self.state = IDLE self.MotorsReady = 0 self.targetRAS = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.targetRAS_angle = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.targetRobot = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.zTransReady = False self.zTrans = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.target = Target() self.connectionStatus = False self.save_position_A = 0 self.save_position_B = 0 self.save_position_C = 0 self.save_position_D = 0
def initialize(self, img, detections): if len(detections) > 0: #self.detector = Resnet() (self.img_height, self.img_width, self.n_channels) = img.shape dPosX = stats.norm( loc = 0.0, scale = self.POS_STD_X ) dPosY = stats.norm( loc = 0.0, scale = self.POS_STD_Y ) dWidth = stats.norm( loc = 0.0, scale = self.SCALE_STD_WIDTH ) dHeight = stats.norm( loc = 0.0, scale = self.SCALE_STD_HEIGHT ) self.persistent_states = np.empty((self.particles_batch * len(detections), self.DIM), dtype = int) idx = 0 for det in detections: self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,0] = det.bbox.x + dPosX.rvs(self.particles_batch) self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,1] = det.bbox.y + dPosY.rvs(self.particles_batch) self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,2] = det.bbox.width + dWidth.rvs(self.particles_batch) self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,3] = det.bbox.height + dHeight.rvs(self.particles_batch) target = Target(det.bbox, idx, (random.randint(0,255), random.randint(0,255), random.randint(0,255))) self.tracks.append(target) self.current_labels.append(idx) self.labels.add(idx) idx+=1 self.persistent_weights = np.ones((self.particles_batch * len(detections)), dtype = float) * ( 1.0/float(self.particles_batch)) #self.tracks_features = self.detector.get_features(img, self.tracks) #self.birth_model = detections self.initialized = True
def create_source_abi_reference_dumps_for_all_products(args): """Create reference ABI dumps for all specified products.""" num_processed = 0 for product in args.products: build_vars = get_build_vars_for_product( ['PLATFORM_VNDK_VERSION', 'BOARD_VNDK_VERSION', 'BINDER32BIT'], product, args.build_variant) platform_vndk_version = build_vars[0] board_vndk_version = build_vars[1] if build_vars[2] == 'true': binder_bitness = '32' else: binder_bitness = '64' chosen_vndk_version = choose_vndk_version(args.version, platform_vndk_version, board_vndk_version) targets = [Target(True, product), Target(False, product)] # Remove reference ABI dumps specified in `args.libs` (or remove all of # them if none of them are specified) so that we may build these # libraries successfully. remove_references_for_all_arches_and_variants(args.ref_dump_dir, chosen_vndk_version, binder_bitness, targets, args.libs) if not args.no_make_lib: # Build all the specified libs, or build `findlsdumps` if no libs # are specified. make_libs_for_product(args.libs, product, args.build_variant, platform_vndk_version, targets) lsdump_paths = read_lsdump_paths(product, args.build_variant, platform_vndk_version, targets, build=False) num_processed += create_source_abi_reference_dumps( args, chosen_vndk_version, binder_bitness, lsdump_paths, targets) return num_processed
def __init__(self): rospy.Subscriber('IGTL_STRING_IN', igtlstring, self.callbackString) rospy.Subscriber('IGTL_TRANSFORM_IN', igtltransform, self.callbackTransformation) self.pub = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) rospy.init_node('talker', anonymous=True) # Define the variables self.TransferData = igtlstring self.CartesianPositionA = 0 self.CartesianPositionB = 0 self.OrientationA = 0 self.OrientationB = 0 self.state = IDLE self.MotorsReady = 0 self.targetRAS = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.targetRAS_angle = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.targetRobot = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.zTransReady = False self.zTrans = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.target = Target()
def initialize(self, img, detections=None, calcHist=False): (self.img_height, self.img_width, self.n_channels) = img.shape self.tracks = [] if len(detections) > 0 and detections: for idx, det in enumerate(detections): target = Target(det.bbox, idx, (random.randint( 0, 255), random.randint(0, 255), random.randint(0, 255)), det.conf, self.SURVIVAL_RATE, det.feature) self.tracks.append(target) self.labels.append(idx) self.initialized = True else: self.initialized = False
def estimate(self, img = None, draw = False, color = (0, 255, 0), verbose = False): if self.initialized: num_estimated = int(round(self.persistent_weights.sum())) if num_estimated > 0: kmeans = KMeans(n_clusters = num_estimated).fit(self.persistent_states) estimated_targets = np.rint(kmeans.cluster_centers_).astype(int) new_tracks = [] label = 0 for et in estimated_targets: while (label in self.labels or label in self.current_labels): label+=1 target = Target(Rectangle(et[0], et[1], et[2], et[3]), label,\ (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255)) ) new_tracks.append(target) self.current_labels.append(label) label+=1 # affinity '''new_tracks_features = self.detector.get_features(img, new_tracks) affinity_matrix = appearance_affinity(self.tracks_features, new_tracks_features) *\ motion_affinity(self.tracks, new_tracks) * \ shape_affinity(self.tracks, new_tracks) affinity_matrix = 1./affinity_matrix row_ind, col_ind = linear_sum_assignment(affinity_matrix)''' ####### cost = cost_matrix(self.tracks, new_tracks) row_ind, col_ind = linear_sum_assignment(cost) for r,c in zip(row_ind, col_ind): new_tracks[c].color = self.tracks[r].color new_tracks[c].label = self.tracks[r].label self.tracks = new_tracks[:] #self.tracks_features = new_tracks_features[:] del self.current_labels[:] for track in self.tracks: self.current_labels.append(track.label) self.labels = self.labels.union(self.current_labels) if draw: for track in self.tracks: cv2.rectangle(img, (track.bbox.x, track.bbox.y), (track.bbox.x + track.bbox.width, track.bbox.y + track.bbox.height), track.color, 3) if verbose: print 'estimated targets: ' + str(num_estimated) return self.tracks
if __name__ == '__main__': try: try: modem_id = int(sys.argv[1]) radius = float(sys.argv[2]) distance_between_meas = int(sys.argv[3]) num_points = float(sys.argv[4]) except IndexError: print('Three arguments are requestd <modem ID>, <circumference radius>, <distance between measurements> and <number of range measurements>') sys.exit() if radius<1 or radius>500: print('Radius must be between 1 and 500') sys.exit() elif num_points<1 or num_points>100: if num_points == -1: print('Infinit number of points will be measured') else: print('Number of measurement must be between 1 and 100') sys.exit() elif distance_between_meas<1 or distance_between_meas>100: print('Distance between measurements must be between 1 and 100') sys.exit() target = Target() tracker = TargetTracking() tracker.start(target, modem_id, radius, distance_between_meas, num_points) print('Done') except rospy.ROSInterruptException or KeyboardInterrupt: pass
class Controller: def __init__(self): # ROS Topics: rospy.Subscriber('IGTL_STRING_IN', igtlstring, self.callbackString) rospy.Subscriber('IGTL_TRANSFORM_IN', igtltransform, self.callbackTransformation) self.pub1 = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) self.pub2 = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) self.motors = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) self.galilStatus = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) rospy.init_node('talker', anonymous=True) # Define the variables for openigtlink self.TransferData1 = igtlstring() self.TransferData1.name = "statusTarget" self.TransferData2 = igtlstring() self.TransferData2.name = "statusZ-Frame" self.motorsData = igtlstring() self.motorsData.name = "motorPosition" self.galilStatusData = igtlstring() self.galilStatusData.name = "status" #Variables: self.status = 0 self.CartesianPositionA = 0 self.CartesianPositionB = 0 self.OrientationA = 0 self.OrientationB = 0 self.state = IDLE self.MotorsReady = 0 self.targetRAS = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.targetRAS_angle = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.targetRobot = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.zTransReady = False self.zTrans = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) self.target = Target() self.connectionStatus = False self.save_position_A = 0 self.save_position_B = 0 self.save_position_C = 0 self.save_position_D = 0 def mm2counts_us_motor(self, distance): return int(US_MM_2_COUNT * distance) def counts2mm_us_motor(self, counts): return int((1.0 / US_MM_2_COUNT) * counts) def mm2counts_piezomotor(self, distance): return int(PE_MM_2_COUNT * distance) def counts2mm_piezomotor(self, counts): return int((1.0 / PE_MM_2_COUNT) * counts) ###################################################### # ROS topic callbacks: ###################################################### def callbackString(self, data): rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.name) if data.name == "INIT": self.state = INIT self.initCondition = data.data[4] + data.data[5] print(self.initCondition) elif data.data == "MOVE": self.state = MOVE elif data.data == "SERIAL": self.reconnect() else: self.state = IDLE rospy.loginfo('Invalid message, returning to IDLE state') def callbackTransformation(self, data): rospy.loginfo(rospy.get_caller_id() + 'I heard ' + data.name) if data.name == "zFrameTransformation": pos = numpy.array([ data.transform.translation.x, data.transform.translation.y, data.transform.translation.z ]) quat = numpy.array([ data.transform.rotation.w, data.transform.rotation.x, data.transform.rotation.y, data.transform.rotation.z ]) self.zTrans = self.quaternion2ht(quat, pos) print(self.zTrans) self.zTransReady = True elif data.name == "targetTransformation": self.state = TARGET pos = numpy.array([ data.transform.translation.x, data.transform.translation.y, data.transform.translation.z ]) quat = numpy.array([ data.transform.rotation.w, data.transform.rotation.x, data.transform.rotation.y, data.transform.rotation.z ]) self.targetRAS = self.quaternion2ht(quat, pos) self.target.set_target_RAS(self.targetRAS) self.targetRAS_angle = self.quaternion2ht(quat, pos) self.target.set_target_RAS_angle(self.targetRAS_angle) elif data.name == "angleTransformation": self.state = TARGET pos = numpy.array([ data.transform.translation.x, data.transform.translation.y, data.transform.translation.z ]) quat = numpy.array([ data.transform.rotation.w, data.transform.rotation.x, data.transform.rotation.y, data.transform.rotation.z ]) self.targetRAS_angle = self.quaternion2ht(quat, pos) self.target.set_target_RAS_angle(self.targetRAS_angle) else: self.state = IDLE rospy.loginfo('Invalid message, returning to IDLE state') ################################################################## def quaternion2ht(self, quat, pos): H = numpy.matrix( '1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0' ) H[0, 3] = pos[0] H[1, 3] = pos[1] H[2, 3] = pos[2] H[0, 0] = 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3]) H[0, 1] = 2.0 * (quat[1] * quat[2] - quat[0] * quat[3]) H[0, 2] = 2.0 * (quat[1] * quat[3] + quat[0] * quat[2]) H[1, 0] = 2.0 * (quat[1] * quat[2] + quat[0] * quat[3]) H[1, 1] = 1.0 - 2.0 * (quat[1] * quat[1] + quat[3] * quat[3]) H[1, 2] = 2.0 * (quat[2] * quat[3] - quat[0] * quat[1]) H[2, 0] = 2.0 * (quat[1] * quat[3] - quat[0] * quat[2]) H[2, 1] = 2.0 * (quat[2] * quat[3] + quat[0] * quat[1]) H[2, 2] = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2]) return H def open_connection(self): try: self.ser = serial.Serial('/dev/ttyUSB0', baudrate=115200, timeout=1) # open serial port #self.ser.open() self.connectionStatus = True print('connection open') return 1 except: try: self.ser = serial.Serial('/dev/ttyUSB1', baudrate=115200, timeout=1) # open serial port #self.ser.open() self.connectionStatus = True print('connection open') return 1 except: self.connectionStatus = False rospy.loginfo( "\n*** could not open the serial communication ***\n") return 0 def reconnect(self): try: self.ser = serial.Serial('/dev/ttyUSB0', baudrate=115200, timeout=1) # open serial port #self.ser.open() self.connectionStatus = True print('connection open') # return 1 except: try: self.ser = serial.Serial('/dev/ttyUSB1', baudrate=115200, timeout=1) # open serial port #self.ser.open() self.connectionStatus = True print('connection open') # return 1 except: self.connectionStatus = False rospy.loginfo( "\n*** could not open the serial communication ***\n") return 0 time.sleep(0.5) self.ser.write(str("DPA=%d\r" % self.save_position_A)) time.sleep(0.1) self.ser.write(str("DPB=%d\r" % self.save_position_B)) time.sleep(0.1) self.ser.write(str("DPC=%d\r" % self.save_position_C)) time.sleep(0.1) self.ser.write(str("DPD=%d\r" % self.save_position_D)) time.sleep(0.1) return 1 def getFTSWstatus(self): # This function asks Galil the footswitch status try: self.ser.flushInput() time.sleep(0.1) self.ser.write(str("MG @IN[1];")) time.sleep(0.1) bytesToRead = self.ser.inWaiting() print(bytesToRead) data_temp = self.ser.read(bytesToRead - 4) print("what do we receive?") print(data_temp) except: print("*** could not send command ***") self.status = 0 self.galilStatusData.data = "No Galil connection" return print("===") print(data_temp) if (float(data_temp) == 1): self.status = 1 self.galilStatusData.data = "FTSW OFF" elif (float(data_temp) == 0): self.status = 2 self.galilStatusData.data = "FTSW ON" else: self.status = 3 self.galilStatusData.data = "Check conection" print(float(data_temp)) return def getMotorPosition(self): try: self.ser.flushInput() time.sleep(0.5) self.ser.write(str("TP;")) time.sleep(0.1) bytesToRead = self.ser.inWaiting() data_temp = self.ser.read(bytesToRead - 3) print(data_temp) except: print("*** could not send command ***") self.status = 0 return str(0) return data_temp def send_movement_in_counts(self, X, Channel): try: self.ser.write(str("PR%s=%d\r" % (Channel, X))) time.sleep(0.1) except: print("*** could not send command ***") return 0 self.ser.flushInput() time.sleep(0.05) self.ser.write(str("PR%s=?\r" % Channel)) time.sleep(0.1) bytesToRead = self.ser.inWaiting() data_temp = self.ser.read(bytesToRead - 3) if int(data_temp) == X: return 1 else: print("*** could not properly send command ***") return 0 def exec_motion(self): try: self.ser.write(str("BG \r")) time.sleep(0.01) self.ser.write(str("PR 0,0,0,0 \r")) except: print("*** could not send command ***") return 0 #TODO: def check_controller(self): try: self.ser.write(str("EO 0\r")) time.sleep(0.1) self.ser.flushInput() self.ser.write(str("MT ?\r")) time.sleep(0.1) MT1 = float(self.ser.read(4)) self.ser.flushInput() self.ser.write(str("MT ,?\r")) time.sleep(0.1) MT2 = float(self.ser.read(4)) self.ser.flushInput() self.ser.write(str("CE ?\r")) time.sleep(0.1) CE1 = float(self.ser.read(2)) self.ser.flushInput() self.ser.write(str("CE ,?\r")) time.sleep(0.1) CE2 = float(self.ser.read(2)) if MT1 == 1.0 and MT2 == 1.0 and CE1 == 0.0 and CE2 == 0.0: return 1 else: rospy.loginfo('Wrong motor or encoder configuration\n') return 0 except: rospy.loginfo("*** could not send command ***") return 0 def SetAbsoluteMotion(self, Channel): try: self.ser.write(str("DP%s=0\r" % Channel)) self.ser.write(str("PT%s=1\r" % Channel)) self.ser.write(str("SH;")) self.AbsoluteMode = True return 1 except: print("*** could not set Absolute mode ***") return 0 def SendAbsolutePosition(self, Channel, X): try: if self.AbsoluteMode: self.ser.write(str("SH;")) time.sleep(0.01) self.ser.write(str("PT%s=1;" % Channel)) time.sleep(0.01) self.ser.write(str("PA%s=%d;" % (Channel, X))) return 1 else: rospy.loginfo("*** PA not available ***") return 0 except: rospy.loginfo("*** could not send command ***") return 0 def init_motors(self): try: if self.initCondition == "US": self.init_us_motors() # First horizontal, second vertical elif self.initCondition == "LT": self.init_piezo("HPELF", "HPEUP") elif self.initCondition == "LC": self.init_piezo("HPELF", "HPEVC") elif self.initCondition == "LB": self.init_piezo("HPELF", "HPEDW") elif self.initCondition == "RT": self.init_piezo("HPERT", "HPEUP") elif self.initCondition == "RC": self.init_piezo("HPERT", "HPEVC") elif self.initCondition == "RB": self.init_piezo("HPERT", "HPEDW") elif self.initCondition == "CT": self.init_piezo("HPEHC", "HPEUP") elif self.initCondition == "CC": self.init_piezo("HPEHC", "HPEVC") elif self.initCondition == "CB": self.init_piezo("HPEHC", "HPEDW") return 1 except: rospy.loginfo("*** could not initialize motors ***") return 0 def init_us_motors(self): try: rospy.loginfo("XQ " + SHARP + "HUSA\n") self.ser.write("XQ " + SHARP + "HUSA;") time.sleep(20.0) rospy.loginfo("XQ " + SHARP + "HUSB\n") self.ser.write("XQ " + SHARP + "HUSB;") rospy.loginfo("DONE INIT...") except: rospy.loginfo("NO INIT...") return def init_piezo(self, positionHorizontal, positionVertical): try: rospy.loginfo(str("XQ " + SHARP + positionVertical + ",0;")) self.ser.write(str("XQ " + SHARP + positionVertical + ",0;")) time.sleep(6.0) rospy.loginfo(str("XQ " + SHARP + positionHorizontal + ",1;")) self.ser.write(str("XQ " + SHARP + positionHorizontal + ",1;")) time.sleep(3.0) rospy.loginfo("*** initialization done***") return 1 except: rospy.loginfo("*** could not initialize Piezo motors ***") return 0 def define_target(self): #Get target (x,y,z) if self.target.ht_RAS_target[ 0, 3] != 0.0 and self.target.ht_RAS_target[ 1, 3] != 0.0 and self.target.ht_RAS_target[2, 3] != 0.0: self.target.define_position_piezo() return self.target.define_target_robot(self.zTrans) else: rospy.loginfo("Please check the target location") return 0
def update(self, img, detections=None, verbose=False, calcHist=False): self.birth_model = [] if self.is_initialized() and len(detections) > 0 and detections: new_detections = [] for idx, det in enumerate(detections): target = Target(bbox = det.bbox, color = (random.randint(0,255), random.randint(0,255), random.randint(0,255)),\ conf = det.conf, survival_rate = self.SURVIVAL_RATE, feature = det.feature) new_detections.append(target) new_tracks = [] if len(self.tracks) > 0: diagonal = np.sqrt( np.power(self.img_height, 2) + np.power(self.img_width, 2)) area = self.img_height * self.img_width cost = cost_matrix(self.tracks, new_detections, diagonal, area, False) #tracks_ind, new_dets_ind = linear_sum_assignment(cost) tracks_ind, new_dets_ind = solve_dense(cost) dets_high_cost = set() for idxTrack, idxNewDet in zip(tracks_ind, new_dets_ind): if cost[idxTrack, idxNewDet] < self.THRESHOLD: new_detections[idxNewDet].label = self.tracks[ idxTrack].label new_detections[idxNewDet].color = self.tracks[ idxTrack].color self.tracks[idxTrack].update( new_detections[idxNewDet].bbox) new_tracks.append(self.tracks[idxTrack]) else: self.tracks[idxTrack].survival_rate = np.exp( self.SURVIVAL_DECAY * (-1.0 + self.tracks[idxTrack].survival_rate * 0.9)) new_tracks.append(self.tracks[idxTrack]) dets_high_cost.add(idxNewDet) tracks_no_selected = set(np.arange(len( self.tracks))) - set(tracks_ind) for idxTrack in tracks_no_selected: #print str(new_tracks[idxTrack].bbox.x) + ',' + str(new_tracks[idxTrack].bbox.y) + ',' + str(new_tracks[idxTrack].bbox.width) + ',' + str(new_tracks[idxTrack].bbox.height) self.tracks[idxTrack].survival_rate = np.exp( self.SURVIVAL_DECAY * (-1.0 + self.tracks[idxTrack].survival_rate * 0.9)) new_tracks.append(self.tracks[idxTrack]) #print '###################' new_detections_no_selected = set(np.arange( len(new_detections))) - set(new_dets_ind) new_detections_no_selected = new_detections_no_selected | dets_high_cost for idxNewDet in new_detections_no_selected: #print str(new_detections[idxNewDet].bbox.x) + ',' + str(new_detections[idxNewDet].bbox.y) + ',' + str(new_detections[idxNewDet].bbox.width) + ',' + str(new_detections[idxNewDet].bbox.height) if np.random.uniform() > self.BIRTH_RATE: new_label = max(self.labels) + 1 new_detections[idxNewDet].label = new_label self.birth_model.append(new_detections[idxNewDet]) self.labels.append(new_label) #print '###################' else: for idxNewDet, det in enumerate(new_detections): if np.random.uniform() > self.BIRTH_RATE: new_label = max(self.labels) + 1 new_detections[idxNewDet].label = new_label self.birth_model.append(new_detections[idxNewDet]) self.labels.append(new_label) #dpp = DPP() #self.tracks = dpp.run(boxes = new_tracks, img_size = (self.img_width, self.img_height),features=None) #self.tracks = nms(new_tracks, 0.7, 0, 0.5) self.tracks = new_tracks
class Controller: def __init__(self): rospy.Subscriber('IGTL_STRING_IN', igtlstring, self.callbackString) rospy.Subscriber('IGTL_TRANSFORM_IN', igtltransform, self.callbackTransformation) self.pub = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10) rospy.init_node('talker', anonymous=True) # Define the variables self.TransferData = igtlstring self.CartesianPositionA = 0 self.CartesianPositionB = 0 self.OrientationA = 0 self.OrientationB = 0 self.state = IDLE self.MotorsReady = 0 self.targetRAS = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.targetRAS_angle = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.targetRobot = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.zTransReady = False self.zTrans = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') self.target = Target() def mm2counts_us_motor(self,distance): # 1 US motor rotation = 500 counts = 0.0998 inches = 2.53492 mm return mm2count*distance def counts2mm_us_motor(self,counts): # 1 US motor rotation = 500 counts = 0.0998 inches = 2.53492 mm return (1.0/mm2count)*counts def mm2counts_piezomotor(self,distance): # Still need to define the best relationship return Piezomm2count*distance def counts2mm_piezomotor(self,counts): # Still need to define the best relationship return (1.0/Piezomm2count)*counts ####################CALLBACKS def callbackString(self, data): rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data) if data.data == "INIT": self.state = INIT elif data.data == "MOVE": self.state = MOVE else: self.state = IDLE rospy.loginfo('Invalid message, returning to IDLE state') def callbackTransformation(self, data): rospy.loginfo(rospy.get_caller_id() + 'I heard') if data.name == "zTrans": pos = numpy.array([data.transform.translation.x,data.transform.translation.y,data.transform.translation.z]) quat = numpy.array([data.transform.rotation.w, data.transform.rotation.x,data.transform.rotation.y,data.transform.rotation.z]) self.zTrans = self.quaternion2ht(quat,pos) self.zTransReady = True elif data.name == "target": self.state = TARGET pos = numpy.array([data.transform.translation.x,data.transform.translation.y,data.transform.translation.z]) quat = numpy.array([data.transform.rotation.w, data.transform.rotation.x,data.transform.rotation.y,data.transform.rotation.z]) self.targetRAS = self.quaternion2ht(quat,pos) self.target.set_target_RAS(self.targetRAS) elif data.name == "angle": self.state = TARGET pos = numpy.array( [data.transform.translation.x, data.transform.translation.y, data.transform.translation.z]) quat = numpy.array([data.transform.rotation.w, data.transform.rotation.x, data.transform.rotation.y, data.transform.rotation.z]) self.targetRAS_angle = self.quaternion2ht(quat, pos) self.target.set_target_RAS_angle(self.targetRAS_angle) else: self.state = IDLE rospy.loginfo('Invalid message, returning to IDLE state') ######################################### def quaternion2ht(self,quat,pos): H = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0') H[0, 3] = pos[0] H[1, 3] = pos[1] H[2, 3] = pos[2] H[0, 0] = 1.0-2.0*(quat[2]*quat[2]+quat[3]*quat[3]) H[0, 1] = 2.0*(quat[1]*quat[2]-quat[0]*quat[3]) H[0, 2] = 2.0*(quat[1]*quat[3]+quat[0]*quat[2]) H[1, 0] = 2.0*(quat[1]*quat[2]+quat[0]*quat[3]) H[1, 1] = 1.0-2.0*(quat[1]*quat[1]+quat[3]*quat[3]) H[1, 2] = 2.0*(quat[2]*quat[3]-quat[0]*quat[1]) H[2, 0] = 2.0*(quat[1]*quat[3]-quat[0]*quat[2]) H[2, 1] = 2.0*(quat[2]*quat[3]+quat[0]*quat[1]) H[2, 2] = 1.0-2.0*(quat[1]*quat[1]+quat[2]*quat[2]) return H def open_connection(self): try: self.ser = serial.Serial('/dev/ttyUSB0', baudrate=115200, timeout=1) # open serial port #self.ser.open() return 1 except: rospy.loginfo("\n*** could not open the serial communication ***\n") return 0 def send_movement_in_counts(self,X,Channel): try: self.ser.write(str("PR%s=%d\r" % (Channel,X))) time.sleep(0.1) except: print("*** could not send command ***") return 0 self.ser.flushInput() time.sleep(0.05) self.ser.write(str("PR%s=?\r" % Channel)) time.sleep(0.1) bytesToRead = self.ser.inWaiting() data_temp = self.ser.read(bytesToRead-3) if int(data_temp) == X: return 1 else: print("*** could not properly send command ***") return 0 def exec_motion(self): try: self.ser.write(str("BG \r")) time.sleep(0.01) self.ser.write(str("PR 0,0,0,0 \r")) except: print("*** could not send command ***") return 0 #TODO: def check_controller(self): try: self.ser.write(str("EO 0\r")) time.sleep(0.1) self.ser.flushInput() self.ser.write(str("MT ?\r")) time.sleep(0.1) MT1 = float(self.ser.read(4)) self.ser.flushInput() self.ser.write(str("MT ,?\r")) time.sleep(0.1) MT2 = float(self.ser.read(4)) self.ser.flushInput() self.ser.write(str("CE ?\r")) time.sleep(0.1) CE1 = float(self.ser.read(2)) self.ser.flushInput() self.ser.write(str("CE ,?\r")) time.sleep(0.1) CE2 = float(self.ser.read(2)) if MT1 == 1.0 and MT2 == 1.0 and CE1 == 0.0 and CE2 == 0.0: return 1 else: rospy.loginfo('Wrong motor or encoder configuration\n') return 0 except: rospy.loginfo("*** could not send command ***") return 0 def SetAbsoluteMotion(self,Channel): try: self.ser.write(str("DP%s=0\r" % Channel)) self.ser.write(str("PT%s=1\r" % Channel)) self.AbsoluteMode = True return 1 except: print("*** could not set Absolute mode ***") return 0 def SendAbsolutePosition(self,Channel,X): try: if self.AbsoluteMode: self.ser.write(str("PA%s=%d\r" % (Channel,X))) return 1 else: print("*** PA not available ***") return 0 except: print("*** could not send command ***") return 0 def init_us_motors(self): try: self.ser.write(str("PR 1000,1000\r")) #Check the values depending on the hardware #self.ser.write(str("BG \r")) time.sleep(1) LRA = 1.0 LRB = 1.0 while LRA==1.0 or LRB==1.0: # Motor A self.ser.flushInput() time.sleep(0.01) self.ser.write(str("MG _LRA\r")) time.sleep(0.01) LRAstring = self.ser.read(4) LRA = float(LRAstring) # Motor B self.ser.flushInput() time.sleep(0.01) self.ser.write(str("MG _LRB\r")) time.sleep(0.01) LRBstring = self.ser.read(4) LRB = float(LRBstring) rospy.loginfo("*** initialization done***") return 1 except: rospy.loginfo("*** could not initialize US motors ***") return 0 def init_piezo(self): try: self.ser.write(str("JG 500 500 500 500\r")) #Set the speed and direction for the first phase of the FI move time.sleep(0.01) self.ser.write(str("HV 300 300 300 300\r")) time.sleep(0.01) self.ser.write(str("FI CD\r")) time.sleep(0.01) self.ser.write(str("BG C\r")) time.sleep(10.0) self.ser.write(str("BG D\r")) time.sleep(10.0) self.ser.write(str("PRC=%d\r" % (PIEZO_INIT_VERTICAL))) time.sleep(5.0) self.ser.write(str("PRD=%d\r" % (PIEZO_INIT_HORIZONTAL))) time.sleep(5.0) rospy.loginfo("*** initialization done***") return 1 except: rospy.loginfo("*** could not initialize Piezo motors ***") return 0 def define_target(self): #Get target (x,y,z) if self.target.HT_RAS_Target[0,3] != 0.0 and self.target.HT_RAS_Target[1,3] != 0.0 and self.target.HT_RAS_Target[2,3] != 0.0: self.target.definePositionPiezo() return self.target.defineTargetRobot(self.zTrans) else: rospy.loginfo( "Please check the target location") return 0