Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import rospy
00019 import threading
00020
00021 from sr_robot_msgs.msg import BiotacAll, ShadowPST, UBI0All
00022
00023 """
00024 Module that receives the tactile values.
00025 """
00026
00027 class TactileReceiver():
00028
00029 def __init__(self):
00030 self.tactile_type = self.find_tactile_type()
00031 self.tactile_state = None
00032
00033 if self.tactile_type == "PST":
00034 self.tactile_listener = rospy.Subscriber("tactile", ShadowPST, self.tactile_callback)
00035 elif self.tactile_type == "biotac":
00036 self.tactile_listener = rospy.Subscriber("tactile", BiotacAll, self.tactile_callback)
00037 elif self.tactile_type == "UBI0":
00038 self.tactile_listener = rospy.Subscriber("tactile", UBI0All, self.tactile_callback)
00039
00040
00041 def find_tactile_type(self):
00042 try:
00043 rospy.wait_for_message("tactile", ShadowPST, timeout = 0.2)
00044 return "PST"
00045 except:
00046 pass
00047
00048 try:
00049 rospy.wait_for_message("tactile", BiotacAll, timeout = 0.2)
00050 return "biotac"
00051 except:
00052 pass
00053
00054 try:
00055 rospy.wait_for_message("tactile", UBI0All, timeout = 0.2)
00056 return "UBI0"
00057 except:
00058 rospy.logwarn("No tactile topic found. This is normal for a simulated hand")
00059
00060 return None
00061
00062 def tactile_callback(self, tactile_msg):
00063 self.tactile_state = tactile_msg
00064
00065 def get_tactile_type(self):
00066 return self.tactile_type
00067
00068 def get_tactile_state(self):
00069 return self.tactile_state