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
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