00001 import re 00002 from threading import Thread 00003 import rostopic 00004 from BAL.Handlers.keepAliveHandler import KeepAliveHandler 00005 from BAL.Header.Requests.PublishRequest import PublishRequest 00006 from BAL.Header.Response.ParamBuildResponse import URF_HRLV 00007 00008 __author__ = 'tom1231' 00009 from BAL.Interfaces.Device import Device 00010 from rospy import Publisher 00011 import rospy 00012 from sensor_msgs.msg import Range 00013 00014 MIN_RANGE_URF_LV_MaxSonar = 0.16 00015 MAX_RANGE_URF_LV_MaxSonar = 6.45 00016 FIELD_OF_VIEW_URF_LV_MaxSonar = 0.7 00017 00018 MIN_RANGE_URF_HRLV_MaxSonar = 0.3 00019 MAX_RANGE_URF_HRLV_MaxSonar = 5.0 00020 FIELD_OF_VIEW_URF_HRLV_MaxSonar = 0.7 00021 00022 class RiCURF(Device): 00023 00024 def __init__(self, devId, param, output): 00025 Device.__init__(self, param.getURFName(devId), output) 00026 self._urfType = param.getURFType(devId) 00027 self._frameId = param.getURFFrameId(devId) 00028 self._pub = Publisher('%s' % self._name, Range, queue_size=param.getURFPubHz(devId)) 00029 #KeepAliveHandler(self._name, Range) 00030 self._devId = devId 00031 00032 self._haveRightToPublish = False 00033 00034 def getType(self): return self._urfType 00035 00036 def publish(self, data): 00037 msg = Range() 00038 msg.header.stamp = rospy.get_rostime() 00039 msg.header.frame_id = self._frameId 00040 if self._urfType == URF_HRLV: 00041 msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar 00042 msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar 00043 msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar 00044 else: 00045 msg.min_range = MIN_RANGE_URF_LV_MaxSonar 00046 msg.max_range = MAX_RANGE_URF_LV_MaxSonar 00047 msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar 00048 msg.radiation_type = Range.ULTRASOUND 00049 msg.range = data 00050 self._pub.publish(msg) 00051 00052 def checkForSubscribers(self): 00053 try: 00054 subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1] 00055 00056 if not self._haveRightToPublish and subCheck == '': 00057 self._output.write(PublishRequest(self.getType(), self._devId, True).dataTosend()) 00058 self._haveRightToPublish = True 00059 00060 elif self._haveRightToPublish and subCheck == 'None': 00061 self._output.write(PublishRequest(self.getType(), self._devId, False).dataTosend()) 00062 self._haveRightToPublish = False 00063 except: pass