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 dopsubscribe(self, data): # subscribe data after exclusion
dop_ = DOP()
dop_ = data
self.HDOPProp = data.HDOP
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()