def __init__(self, ):
self.reserve = 0
self.GrouLat_ = 0.0
self.GrouLon_ = 0.0
self.GrouAlt_ = 0.0
self.calAngNNLOS = 0.0
self.SPANUpdate = 0.0
self.busUpdate = 0.0
self.GNSSUpdate = 0.0 # Ublox
self.SPANYawUpdate = 0.0
self.posArr = []
self.excluSatLis = [19] # 23,26,93 for big nlos, experiment 2 (alway visible: 13 26 31 93 96 100 sometimes visible: 23) Low elevation:3,91,22,32,94,
self.GNSS_ = GNSS_Raw_Array()
self.GNSSNRAWlosDel = GNSS_Raw_Array()
self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal()
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
rospy.Subscriber('/novatel_data/bestpos', BESTPOS, self.callcptBestPos_) # Ground truth
rospy.Subscriber('double_decker_parameters', PoseArray, self.callDouDeckBus) # bus
rospy.Subscriber('GNSS_', GNSS_Raw_Array, self.CallGNSS_) # Ublox
rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.callINSPVAXNLOS_) # yaw
rospy.Subscriber('velodyne_points_0', PointCloud2, self.PointCloudCall) # velodyne
rospy.Subscriber('GNSSRAWNlosDel_', GNSS_Raw_Array, self.CallGNSSRAWNlosDel_) # Ublox
self.GNSS_Navfix_pub = rospy.Publisher('GNSS_NavFix', NavSatFix,queue_size=100) #
self.propGNSS_Navfix_pub = rospy.Publisher('GNSS_NavFix_pro', NavSatFix, queue_size=100) #
def __init__(self, ):
self.reserve = 0
self.GrouLat_ = 0.0
self.GrouLon_ = 0.0
self.GrouAlt_ = 0.0
self.calAngNNLOS = 0.0
self.SPANUpdate = 0.0
self.busUpdate = 0.0
self.GNSSUpdate = 0.0 # Ublox
self.SPANYawUpdate = 0.0
self.GNSSNRAWlosDelTimeFlag = 0
self.posArr = []
self.excluSatLis = [17,28,8,3,22,9,92] # Not blocked: 11,1,93,94,96,89,7 blocked: 17,28,8,3,22,9,92
self.GNSS_ = GNSS_Raw_Array()
self.GNSSNRAWlosDel = GNSS_Raw_Array()
self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal()
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
rospy.Subscriber('/novatel_data/bestpos', BESTPOS, self.callcptBestPos_) # Ground truth
rospy.Subscriber('double_decker_parameters', PoseArray, self.callDouDeckBus) # bus
rospy.Subscriber('GNSS_', GNSS_Raw_Array, self.CallGNSS_) # Ublox
rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.callINSPVAXNLOS_) # yaw
rospy.Subscriber('velodyne_points_0', PointCloud2, self.PointCloudCall) # velodyne
rospy.Subscriber('GNSSRAWNlosDel_', GNSS_Raw_Array, self.CallGNSSRAWNlosDel_) # Ublox
self.GNSS_Navfix_pub = rospy.Publisher('GNSS_NavFix', NavSatFix,queue_size=100) #
self.propGNSS_Navfix_pub = rospy.Publisher('GNSS_NavFix_pro', NavSatFix, queue_size=100) #
self.graphslam_Navfix_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=100) #
self.graphslam_GeoPoint_pub = rospy.Publisher('/gps/geopoint', GeoPointStamped, queue_size=100) #
self.graphslam_PointCloud_pub = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=100) #
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu', self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
geopoint.position.latitude = float(llh_[0])
geopoint.position.longitude = float(llh_[1])
geopoint.position.altitude = float(llh_[2])
self.graphslam_GeoPoint_pub.publish(geopoint)
def nlosExclusionF(self): # function
# print len(self.posArr),len(self.GNSS_.GNSS_Raws)
if (len(self.posArr) > 0) and (len(self.GNSS_.GNSS_Raws) > 0):
# print 'both double-decker bus and GNSS_ are ready...'
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_,'statAuto',self.excluSatLis)
self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal()
# self.puGNSSPosCalF_.iterPosCal(self.GNSS_,'LSGNSS')
self.puGNSSPosCalF_.iterPosCal(self.nlosExclusionF_.IniGNSS_, 'WLSGNSS')
self.puGNSSPosCalF2_ = puGNSSPosCal.GNSSPosCal()
self.puGNSSPosCalF2_.iterPosCal(self.nlosExclusionF_.nlosGNSSDel_, 'WLSGNSS')
self.posArr[:] = []
self.GNSS_.GNSS_Raws[:] = []
def PointCloudCall(
self, data
): # 20 Hz As this function has highest frequency, good for update
self.pointCloud_ = PointCloud2()
self.pointCloud_ = data
# self.graphslam_PointCloud_pub.publish(self.pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_,
'statManu_ExclusionOpenLoop',
self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if ((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1)
and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time !=
self.GNSSNRAWlosDelTimeFlag)
): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = self.pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
# calculate the ENU coordiantes initialLLH
enu_ = pugeomath.transformEcefToEnu(self.initialLLH,
self.puGNSSPosCalF_prop.ecef_)
print 'enu_ ->: ', enu_
geopoint.position.latitude = float(enu_[0])
geopoint.position.longitude = float(enu_[1])
# geopoint.position.altitude = float(llh_[2]) # use this to save the covariance in altitude component
geopoint.position.altitude = self.GNSSNRAWlosDel.GNSS_Raws[
-1].snr * self.HDOPProp # save the covariance
self.graphslam_GeoPoint_pub.publish(geopoint)
def __init__(self, parent=None):
QtCore.QThread.__init__(self, parent)
self.posArr = []
self.GNSS_ = GNSS_Raw_Array()
self.print_ = 8
self.iniSatLis_ = []
self.iniSnr = []
# self.excluSatLis = [29,23,92] # for small nlos, experiment 1
self.excluSatLis = [
3, 91, 22, 32, 94, 23, 26, 93
] # 23,26,93 for big nlos, experiment 2 (alway visible: 13 26 31 93 96 100 sometimes visible: 23) Low elevation:3,91,22,32,94,
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion()
self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal()
self.puGNSSPosCalF2_ = puGNSSPosCal.GNSSPosCal()
rospy.Subscriber('double_decker_parameters', PoseArray,
self.callDouDeckBus)
rospy.Subscriber('GNSS_', GNSS_Raw_Array, self.CallGNSS_)
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statAuto', self.excluSatLis)
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if(len(self.GNSSNRAWlosDel.GNSS_Raws) > 1):
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)