tactile_receiver.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 # Copyright 2014 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
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


sr_hand
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:08:51