Go to the documentation of this file.00001
00002 import roslib; roslib.load_manifest('hrl_fabric_based_tactile_sensor')
00003 import rospy
00004 from hrl_msgs.msg import FloatArray
00005
00006 import hrl_lib.util as ut
00007
00008 import hrl_fabric_based_tactile_sensor.adc_publisher_node as apn
00009
00010 from m3skin_ros.msg import RawTaxelArray
00011 from geometry_msgs.msg import Transform
00012
00013 from m3skin_ros.srv import None_TransformArray, None_TransformArrayResponse
00014 from m3skin_ros.srv import None_String
00015
00016 class Fabric_Skin_Patch():
00017 def __init__(self):
00018 self.n_taxels_x = 5
00019 self.n_taxels_y = 3
00020 self.taxel_dim_x = 0.05
00021 self.taxel_dim_y = 0.05
00022
00023 self.link_name = '/fabric_skin_link'
00024 self.tar = None_TransformArrayResponse()
00025 for i in range(self.n_taxels_x):
00026 for j in range(self.n_taxels_y):
00027 t = Transform()
00028 t.translation.x = i*self.taxel_dim_x
00029 t.translation.y = j*self.taxel_dim_y
00030 t.translation.z = 0.
00031
00032 t.rotation.x = 0
00033 t.rotation.y = 0
00034 t.rotation.z = 0
00035 t.rotation.w = 1
00036 self.tar.data.append(t)
00037
00038 def local_coord_frames_cb(self, req):
00039 return self.tar
00040
00041 def link_name_cb(self, req):
00042 return self.link_name
00043
00044
00045 if __name__ == '__main__':
00046 dev_name = '/dev/ttyACM0'
00047 baudrate = 115200
00048 serial_dev = apn.setup_serial(dev_name, baudrate)
00049
00050 raw_data_pub = rospy.Publisher('/fabric_skin/taxels/raw_data',
00051 RawTaxelArray)
00052
00053 fsp = Fabric_Skin_Patch()
00054 d = ut.load_pickle('taxel_registration_dict.pkl')
00055 fsp.link_name = d['tf_name']
00056 fsp.tar = d['transform_array_response']
00057
00058
00059 rospy.Service('/fabric_skin/taxels/srv/local_coord_frames',
00060 None_TransformArray, fsp.local_coord_frames_cb)
00061 rospy.Service('/fabric_skin/taxels/srv/link_name', None_String,
00062 fsp.link_name_cb)
00063
00064 rospy.init_node('fabric_skin_driver_node')
00065
00066 for i in range(10):
00067 ln = serial_dev.readline()
00068
00069 rospy.loginfo('Started publishing data')
00070
00071 rta = RawTaxelArray()
00072 while not rospy.is_shutdown():
00073 rta.val_z = apn.get_adc_data(serial_dev)[0:15]
00074 raw_data_pub.publish(rta)
00075
00076 serial_dev.close()
00077
00078