00001 import re 00002 from threading import Thread 00003 import rospy 00004 import rostopic 00005 from BAL.Handlers.keepAliveHandler import KeepAliveHandler 00006 from BAL.Header.Requests.PublishRequest import PublishRequest 00007 from BAL.Header.Response.ParamBuildResponse import Button 00008 00009 __author__ = 'tom1231' 00010 from rospy import Publisher 00011 from BAL.Interfaces.Device import Device 00012 from std_msgs.msg import Bool 00013 00014 00015 class RiCSwitch(Device): 00016 def __init__(self, devId,param, output): 00017 Device.__init__(self, param.getSwitchName(devId), output) 00018 self._pub = Publisher('%s' % self._name, Bool, queue_size=param.getSwitchPubHz(devId)) 00019 self._switchId = devId 00020 self._haveRightToPublish = False 00021 # KeepAliveHandler(self._name, Bool) 00022 00023 def publish(self, data): 00024 msg = Bool() 00025 msg.data = data 00026 self._pub.publish(msg) 00027 00028 def checkForSubscribers(self): 00029 try: 00030 subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1] 00031 00032 if not self._haveRightToPublish and subCheck == '': 00033 self._output.write(PublishRequest(Button, self._switchId, True).dataTosend()) 00034 self._haveRightToPublish = True 00035 00036 elif self._haveRightToPublish and subCheck == 'None': 00037 self._output.write(PublishRequest(Button, self._switchId, False).dataTosend()) 00038 self._haveRightToPublish = False 00039 except: pass 00040 00041 def getType(self): return Button