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 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 GNSSNavPropCall(self,data): # llh from proposed method # about 20 Hz
self.navfixprop_ = NavSatFix()
self.navfixprop_ = data
self.latProp_.append(self.navfixprop_.latitude)
self.lonProp_.append(self.navfixprop_.longitude)
self.navfixpropAvailable = 1
# ---sometimes, self.GNSSNRAWlosDel will be initialized by function CallGNSSRAWNlosDel_1
# ---In this case, save this value for further processing
self.GNSSNRAWlosDelSave = GNSS_Raw_Array()
self.GNSSNRAWlosDelSave = self.GNSSNRAWlosDel
if((self.navfixprop_.header.stamp.secs == self.navfixTruth_.header.stamp.secs) and (len(self.GNSSNRAWlosDel.GNSS_Raws))):
# print 'self.navfixprop_.header.stamp.secs',self.navfixprop_.header.stamp.secs
if(self.firstTimeProp_ == 0):
self.firstTimeProp_ = float(self.navfixprop_.header.stamp.secs)
self.GNSSTimeProp.append((float(self.navfixprop_.header.stamp.secs)- float(self.firstTimeProp_)) / 1000) # get GNSS Time
self.SatNumProp.append(float(len(self.GNSSNRAWlosDel.GNSS_Raws))) # get SatNum
puGNSSPosCalProp_ = puGNSSPosCal.GNSSPosCal()
self.HDOPProp.append(float(puGNSSPosCalProp_.DopCalculation(self.GNSSNRAWlosDelSave)))
self.errorProp.append(self.PosErrorCal(self.navfixTruth_, self.navfixprop_))
errorxyz_ = Error()
errorxyz_.header.stamp.secs = self.navfixprop_.header.stamp.secs
errorxyz_.errorxyz = float(self.errorProp[-1])
self.ErrorProp_Pub.publish(errorxyz_)
Dop_ = DOP()
Dop_.header.stamp.secs = self.navfixprop_.header.stamp.secs
Dop_.HDOP = float(self.HDOPProp[-1])
self.DOPProp_Pub.publish(Dop_)
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 __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 CallGNSS_(self, data): # GNSS data # 1Hz
self.GNSS_ = data
self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal()
self.puGNSSPosCalF_.iterPosCal(self.GNSS_, 'WLSGNSS')
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_.ecef_)
navfix_.header.stamp.secs = self.GNSS_.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.GNSS_Navfix_pub.publish(navfix_) # print 'ecef',self.puGNSSPosCalF_.ecef_,'llh',llh_
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 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_)
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
if((self.navfixAvailable) and (len(self.GNSS_.GNSS_Raws)) and (self.navfixTruth_.header.stamp.secs == self.navfix_.header.stamp.secs) and (self.navfixTruth_.header.stamp.secs == self.GNSS_.GNSS_Raws[-1].GNSS_time)):
# print 'three conventional source ok at GPS_Week_Second=',self.navfix_.header.stamp.secs
self.navfixAvailable = 0
if(self.firstTime_ == 0): # save first epoch
self.firstTime_ = self.navfix_.header.stamp.secs
self.GNSSTime.append((float(self.navfix_.header.stamp.secs) - float(self.firstTime_)) / 1000)
self.SatNum.append(float(len(self.GNSS_.GNSS_Raws)))
puGNSSPosCal_ = puGNSSPosCal.GNSSPosCal()
self.HDOP.append(float(puGNSSPosCal_.DopCalculation(self.GNSS_)))
self.error.append(self.PosErrorCal(self.navfixTruth_,self.navfix_))
errorxyz = Error()
errorxyz.header.stamp.secs = self.navfix_.header.stamp.secs
errorxyz.errorxyz = float(self.error[-1])
self.Error_Pub.publish(errorxyz)
Dop_ = DOP()
Dop_.header.stamp.secs = self.navfix_.header.stamp.secs
Dop_.HDOP = float(self.HDOP[-1])
self.DOP_Pub.publish(Dop_)
if ((len(self.SatNum)) and (len(self.SatNumProp))):
print 'NLOS exclusion available',self.SatNum[-1],self.SatNumProp[-1],self.SatNum[-1] - self.SatNumProp[-1]
self.errorEv.append(float(self.error[-1]))
self.errorPropEv.append(float(self.errorProp[-1]))
meanEr_ = []
std_ = np.std(self.errorEv) # stdandard deviation
print 'meanEr_', sum(self.errorEv) / (len(self.errorEv)), 'std_----', std_
self.PercentCal(self.errorEv)
stdProp_ = np.std(self.errorPropEv)
print 'meanErPros_', sum(self.errorPropEv) / (len(self.errorPropEv)), 'stdProp_', stdProp_
self.PercentCal(self.errorPropEv)
print '----------------------------------------'
# self.busAvailProp.append(float(30))
# else:
# self.busAvailProp.append(float(10))
# if(len(self.SatNum) < 15):
# self.busAvail.append(10)
# if ((len(self.SatNum) >= 15) and (len(self.SatNum) <87)):
# self.busAvail.append(30)
# if (len(self.SatNum) >= 87):
# self.busAvail.append(10)
self.busAvail.append(1 * (puBusDetectionLabelling1.truth[len(self.SatNum)] * 1)+0)
self.busAvailProp.append(1 * (puBusDetectionLabelling1.detection[len(self.SatNum)] * 1) + 0)
self.axesCurv_1.tick_params(axis='both', labelsize=25)
self.axesCurv_2.tick_params(axis='both', labelsize=25)
# self.axesCurv_2.set_ylim([-2, 2])
self.axesCurv_3.tick_params(axis='both', labelsize=25)
# plot satellite number
# plot satellite number
# self.axesCurv_2.plot(self.GNSSTime, self.SatNum, '-*', linewidth='1', color='red', label='SatNum')
if (len(self.GNSSTimeProp) == len(self.SatNumProp)):
self.axesCurv_2.plot(self.GNSSTimeProp, self.SatNumProp, '-*', linewidth='1', color='blue',
label='SatNumProp')
# plot HDOP or exclusion accuracy
# self.axesCurv_2.plot(self.GNSSTime, self.HDOP, '-*', linewidth='1', color='dodgerblue', label='HDOP')
# if (len(self.GNSSTimeProp) == len(self.HDOPProp)):
# self.axesCurv_2.plot(self.GNSSTimeProp, self.HDOPProp, '-*', linewidth='1', color='lime',label='HDOPProp')
self.axesCurv_1.plot(self.GNSSTime, self.busAvail, '-*', linewidth='1', color='red', label='busAvail')
self.axesCurv_1.plot(self.GNSSTime, self.busAvailProp, '-*', linewidth='1', color='blue',
label='busAvailProp')
# plot error
self.axesCurv_3.plot(self.GNSSTime, self.error, '-*', linewidth='1', color='red', label='HDOP')
if (len(self.GNSSTimeProp) == len(self.errorProp)):
self.axesCurv_3.plot(self.GNSSTimeProp, self.errorProp, '-*', linewidth='1', color='blue',
label='errorProp')
self.canvas.draw()
self.axesCurv_1.clear()
self.axesCurv_2.clear()
self.axesCurv_3.clear()
