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()