Go to the documentation of this file.00001 import re
00002 from threading import Thread
00003 import rospy
00004 import rostopic
00005 import time
00006 from BAL.Handlers.keepAliveHandler import KeepAliveHandler
00007 from BAL.Header.Requests.PublishRequest import PublishRequest
00008 from BAL.Header.Response.ParamBuildResponse import GPS
00009
00010 __author__ = 'tom1231'
00011 from rospy import Publisher
00012 from sensor_msgs.msg import NavSatFix
00013 from BAL.Interfaces.Device import Device
00014
00015 GPS_WD_TIMEOUT = 300
00016
00017 class RiCGPS(Device):
00018 def __init__(self, param, output):
00019 Device.__init__(self, param.getGpsName(), output)
00020 self._frameId = param.getGpsFrameId()
00021 self._pub = Publisher('%s' % self._name, NavSatFix, queue_size=param.getGpsPubHz())
00022 self._haveRightToPublish = False
00023 self._old_fix = 0
00024 self._wd = int(round(time.time() * 1000))
00025 KeepAliveHandler(self._name, NavSatFix)
00026
00027 def publish(self, data):
00028 now = int(round(time.time() * 1000))
00029 msg = NavSatFix()
00030 msg.header.frame_id = self._frameId
00031 msg.header.stamp = rospy.get_rostime()
00032 msg.latitude = data.getLat()
00033 msg.longitude = data.getLng()
00034 msg.altitude = data.getMeters()
00035 cur_fix = data.getFix()
00036
00037 if (cur_fix != self._old_fix):
00038 msg.status.status = 0
00039 self._old_fix = cur_fix
00040 self._wd = now
00041 elif (now - self._wd) < GPS_WD_TIMEOUT:
00042 msg.status.status = 0
00043 else:
00044 msg.status.status = -1
00045 msg.position_covariance_type = 1
00046 msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
00047 msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
00048 msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
00049 msg.status.service = 1
00050 self._pub.publish(msg)
00051
00052 def getType(self): return GPS
00053
00054 def checkForSubscribers(self):
00055 try:
00056 subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
00057
00058 if not self._haveRightToPublish and subCheck == '':
00059 self._output.write(PublishRequest(GPS, 0, True).dataTosend())
00060 self._haveRightToPublish = True
00061
00062 elif self._haveRightToPublish and subCheck == 'None':
00063 self._output.write(PublishRequest(GPS, 0, False).dataTosend())
00064 self._haveRightToPublish = False
00065 except: pass