# create node
rospy.init_node('force_polaris_lc', anonymous=True)
# create publishers
pub_fx = rospy.Publisher('/force_polaris_lc/force_x',
Float32,
queue_size=1)
pub_fy = rospy.Publisher('/force_polaris_lc/force_y',
Float32,
queue_size=1)
pub_fz = rospy.Publisher('/force_polaris_lc/force_z',
Float32,
queue_size=1)
# create classes
lc = utilities.ZLCdataFromADC(0)
opt_track = utilities.OpticalTracker()
rate = rospy.Rate(500)
# load transformation matrix from npz file
npzfile = np.load("transformation_matrix.npz")
transform_camera_to_robot = npzfile['transform']
print 'trans matrix', transform_camera_to_robot
npz_lc_data = np.load("load_cell_linear_equation_parameters.npz")
lc_lin_eq_param = npz_lc_data['lc_equation_parameters']
while opt_track.get_ot_data() is None:
print 'No optical data yet'
if opt_track.get_ot_data() is not None:
if __name__ == '__main__':
# create node
rospy.init_node('force_feedback', anonymous=True)
# Create a Python proxy for PSM2, name must match ros namespace
p = dvrk.psm('PSM1')
rate = rospy.Rate(500)
# instantiating classes
x_adc = utilities.XYdataFromADC(1)
y_adc = utilities.XYdataFromADC(0)
z_adc = utilities.ZLCdataFromADC(1)
z_joint = p.get_current_joint_effort()[2]
# # load linear equations parameters
load_lin_eq_param = np.load("calibration_equation.npz")
a_x_eq_param = load_lin_eq_param['a_x']
a_y_eq_param = load_lin_eq_param['a_y']
# create publishers
pub_fx = rospy.Publisher('/force_feedback/force_x', Float32, queue_size=1)
pub_fy = rospy.Publisher('/force_feedback/force_y', Float32, queue_size=1)
pub_fz = rospy.Publisher('/force_feedback/force_z', Float32, queue_size=1)
# get reading when force is 0
for i in range(0, 300):
deq_adc_x.append(x_adc.get_value())