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