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 callcptBestPos_(self, data):
self.bestPos_ = BESTPOS()
self.bestPos_ = data
self.GNSSArr1 = GNSS_Raw_Array()
pos_index = 0
if (self.endFlag == 1):
if ((self.bestPos_.header.gps_week_seconds) in self.GNSS_time):
print 'GNSS_time.index(self.bestPos_.header.gps_week_seconds)', self.bestPos_.header.gps_week_seconds
print self.GNSS_time.index(
self.bestPos_.header.gps_week_seconds)
print '-------------------------------------------'
pos_index = self.GNSS_time.index(
self.bestPos_.header.gps_week_seconds)
totalSv_ = self.total_sv[pos_index]
self.GNSS_1 = GNSS_Raw()
for index_ in range(int(totalSv_)):
index_ = index_ + pos_index
print 'index_', index_
self.GNSS_1.GNSS_time = float(
self.GNSS_time[index_]
) # GNSS time contained in csv file
self.GNSS_1.total_sv = float(
self.total_sv[index_]
) # total sitellites in one epoch (epoch: each time point)
self.GNSS_1.prn_satellites_index = float(
self.prn_satellites_index[index_])
self.GNSS_1.pseudorange = float(self.pseudorange[index_])
self.GNSS_1.snr = float(
self.snr[index_]
) # signal noise ratio for this satellite
self.GNSS_1.elevation = float(
self.elevation[index_]
) # elevation for this satellite and receiver
self.GNSS_1.azimuth = float(
self.azimuth[index_]) # azimuth for this satellite
self.GNSS_1.err_tropo = float(
self.err_tropo[index_]) # troposphere error in meters
self.GNSS_1.err_iono = float(
self.err_iono[index_]) # ionophere error in meters
self.GNSS_1.sat_clk_err = float(
self.sat_clk_err[index_]
) # satellite clock bias caused error
self.GNSS_1.sat_pos_x = float(
self.sat_pos_x[index_]
) # satellite positioning in x direction
self.GNSS_1.sat_pos_y = float(
self.sat_pos_y[index_]
) # satellite positioning in y direction
self.GNSS_1.sat_pos_z = float(
self.sat_pos_z[index_]
) # satellite positioning in Z direction
if (self.GNSS_1.elevation > 18.0): # mask angle = 18
self.GNSSArr1.GNSS_Raws.append(self.GNSS_1)
self.GNSS_1 = GNSS_Raw()
self.GNSSArr1.header.seq = self.seq
self.GNSSArr1.header.stamp.secs = self.stampSecs
self.GNSSArr1.header.stamp.nsecs = self.stampnSecs
self.GNSSArr1.header.frame_id = 'GNSS_'
self.GNSS_Raw_pub.publish(self.GNSSArr1)
def DopCalculation(self,GNSS_one_epoch_): # get Dop in one epoch
GNSS_one_epoch = GNSS_Raw_Array()
GNSS_one_epoch = GNSS_one_epoch_
H_matrix_ = array([2, 2, 2, 1], dtype='float') # creat H matrix to save transform parameters
Hdop_ = 0.0 # create an variable to save Hdop
elemin = 15.0 # minimun elevation angle
for index_1 in range(len(GNSS_one_epoch.GNSS_Raws)): # index all the satellite information in one epoch
if (GNSS_one_epoch.GNSS_Raws[index_1].elevation <= elemin):
# print 'satellite elevation less than 15 degree=',GNSS_one_epoch.GNSS_Raws[index_1].elevation
continue
cosel = float(cos(GNSS_one_epoch.GNSS_Raws[index_1].elevation))
sinel = float(sin(GNSS_one_epoch.GNSS_Raws[index_1].elevation))
H_row = [] # H matrix row
H_row.append(float(cosel * sin(GNSS_one_epoch.GNSS_Raws[index_1].azimuth)))
H_row.append(float(cosel * cos(GNSS_one_epoch.GNSS_Raws[index_1].azimuth)))
H_row.append(float(sinel))
H_row.append(1.0)
H_matrix_ = np.row_stack((H_matrix_, H_row)) # add each row to H_matrix
del H_row[:] # relief H_row
# get H matrix
H_matrix_ = np.delete(H_matrix_, [0], axis=0) # delete the first row of H matrix
# print 'H_matrix_',H_matrix_
Q_matrix_ = np.dot((H_matrix_.transpose()), H_matrix_) # H(T) * G
Q_matrix_ = np.linalg.inv(Q_matrix_) # inverse matrix of H(T) * G
Hdop = float(sqrt(Q_matrix_[0, 0] + Q_matrix_[1, 1]))
# print 'Q_matrix_', Q_matrix_, 'Hdop', Hdop
return float(Hdop) # return result
def CallGNSS_1(self, data): # GNSS data
self.GNSS_1 = GNSS_Raw_Array()
self.GNSS_1 = data
self.satecirclRad = 3
for index_1 in range(len(self.GNSS_1.GNSS_Raws)): # index all the satellite information in one epoch
self.azim_.append( (self.GNSS_1.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.cos(-1*(self.GNSS_1.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0))
self.elev_.append( (self.GNSS_1.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.sin(-1*(self.GNSS_1.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0))
self.satIdx.append(self.GNSS_1.GNSS_Raws[index_1].prn_satellites_index)
self.drawAll()
def __init__(self):
# rospy.init_node('csv2Topi_', anonymous=True) # initialize node with specific node name
self.GNSS_Raw_pub = rospy.Publisher(
'GNSS_', GNSS_Raw_Array,
queue_size=100) # customerized GNSS raw data ros message
rospy.Subscriber('/novatel_data/bestpos', BESTPOS,
self.callcptBestPos_)
rospy.Subscriber('/imu_c', Imu, self.callcptImu)
self.lowEleSatLis_ = [3, 91, 22, 32, 94]
self.lowEleSatLis_ = []
self.eleThres_ = 18.0 # 18
self.snrThres_ = 20.0 # 20
self.GNSSArr_ = GNSS_Raw_Array()
self.initVariable()
def __init__(self, csvPath, tophz_):
# rospy.init_node('csv2Topi_', anonymous=True) # initialize node with specific node name
self.GNSS_Raw_pub = rospy.Publisher(
'GNSS_', GNSS_Raw_Array,
queue_size=10) # customerized GNSS raw data ros message
self.topHz = tophz_
self.lowEleSatLis_ = [3, 91, 22, 32, 94]
self.lowEleSatLis_ = []
self.eleThres_ = 18.0 # 18
self.snrThres_ = 20.0 # 20
self.csvPath = csvPath # /home/wenws/3_nlosMovobj/src/nlosExclusion/src/data5/exper1/sv_data.csv
if (tophz_ > 0):
self.readCSV()
self.GNSSArr_ = GNSS_Raw_Array()
def __init__(self, calAngle):
self.nlExcMode = 'statManu' # 'statAuto' 'dynaAuto' 'statManu' 'dynaManu'
self.GNSSNlos_pub = rospy.Publisher(
'GNSSRAWNNlos_', GNSS_Raw_Array,
queue_size=10) # customerized GNSS raw data ros message
self.GNSSNlosDel_pub = rospy.Publisher(
'GNSSRAWNlosDel_', GNSS_Raw_Array,
queue_size=10) # customerized GNSS raw data ros message
rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.callINSPVAX_)
self.exclusionSatnum_Pub = rospy.Publisher('exclusionSatNum',
exclusionSatNum,
queue_size=100) #
self.calAngN = calAngle
self.bouRang = 160
self.GNSSTime = 0
self.azim_ = []
self.elev_ = []
self.azimIni_ = []
self.elevIni_ = []
self.satIdx = []
self.bouSide1X = []
self.bouSide1Y = []
self.bouSide2X = []
self.bouSide2Y = []
self.bouSide1IniX = []
self.bouSide2IniX = []
self.satExcl_ = []
self.mannuSat = []
self.GPSNum_ = 0
self.BeiNum_ = 0
self.GPSExclNum = 0
self.BeidExclNum = 0
self.IniGNSS_ = GNSS_Raw_Array()
self.nlosGNSS_ = GNSS_Raw_Array()
self.posArr = [
] # create a list to save double-decker bus boundary information
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.initialLLH = [114.19289862,22.3181226456,0]
self.HDOPProp = 0
# 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.excluSatLis = [9,14,31] # 9,14,31,94,90,87
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
rospy.Subscriber('DOP_', DOP, self.dopsubscribe) # 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 __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 callback_GNSS_Raw_data(data): # callback function to handle GNSS Raw data
GNSS_Raw_array_list=[] # create a list to save PoseArray message, which contains GNSS Raw data
GNSS_exclusion_Array_1 = GNSS_Raw_Array() # create an variable in GNSS_Raw_Array format to save GNSS data after exclusion
GNSS_Raw_array_list=data.GNSS_Raws # save all the satellite azimuth information and elevation information in GNSS_Raw_array_list (one GNSS_Raw_array_list message contains several GNSS_Raws message)
GNSS_exclusion_Array_1=data
plot_total_sate.append(GNSS_exclusion_Array_1.GNSS_Raws[0].total_sv) # save the total satellites numbers (GPS+Beidou)
plot_total_sateTim.append(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))
GPSNum = 0 # create a variable to save GPS number
BeidouNum = 0 # create a variable to save Beidou number
for i in range(len(GNSS_Raw_array_list)): # index all the Pose message in PoseArray message (http://docs.ros.org/api/geometry_msgs/html/msg/PoseArray.html)
# x--azimuth y--elevation (Skyplot: https://github.com/feddischson/skyplotwidget)
satellite_azimuth_list.append( (GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.cos(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0)) # function to transfer from azimuth into skyplot azimuth
satellite_elevation_list.append( (GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.sin(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0)) # function to transfer from elevation into skyplot elevation
satellite_azimuth_list_glob.append((GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.cos(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0))
satellite_elevation_list_glob.append((GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.sin(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0))
satellite_number_list.append(GNSS_Raw_array_list[i].prn_satellites_index)
satellite_number_list_glob.append(GNSS_Raw_array_list[i].prn_satellites_index)
satellite_initial_azimuth_list.append(GNSS_Raw_array_list[i].azimuth) # save initial azimuth data for plotting in callback_azi_eleva()
satellite_initial_azimuth_list_glob.append(GNSS_Raw_array_list[i].azimuth) # save initial azimuth data for NLOS exclusion in callback_GNSS_Raw_data
if ((GNSS_Raw_array_list[i].prn_satellites_index >= 1) and (GNSS_Raw_array_list[i].prn_satellites_index <= 32)): # GPS
GPSNum=GPSNum+1
if ((GNSS_Raw_array_list[i].prn_satellites_index >= 88) and (GNSS_Raw_array_list[i].prn_satellites_index <= (87+37))): # Beidou
BeidouNum=BeidouNum+1
if (GPSNum > 0): # make sure that GPSNum is not zero
plot_GPS_sate.append(float(GPSNum)) # save GPS number to list
plot_Beidou_sate.append(float(BeidouNum)) # save Beidou number to list
plot_Beidou_sateTim.append(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))
GPSNum = 0 # clear
BeidouNum = 0 # clear
#NLOS Exclusion
len_listx_2 = len(listx_2_ini_glob)
if (len_listx_2 >= 3): # set threshold: maxinum double-decker bus for NLOS exclusion
len_listx_2 = 3
if(exclusMode==0): # autonomatic exclusion
for i_bou_num in range(len_listx_2): # index all the boundary line (usually only one double-decker bus, sometime 2)
for index_sat in range(len(GNSS_Raw_array_list)): # index all the satellite information in one epoch
point0_x = 0.0 # yuanxin x
point0_y = 0.0 # yuanxin y
point1_x=float(listx_2_glob[i_bou_num]) # point1 coordinate x
point1_y=float(listy_2_glob[i_bou_num]) # point1 coordinate y
point2_x=float(listx_3_glob[i_bou_num]) # point2 coordinate x
point2_y=float(listy_3_glob[i_bou_num]) # point2 coordinate y
point_x =float(satellite_azimuth_list_glob[index_sat]) # point_x (satellite azimuth in skyplot)
point_y =float(satellite_elevation_list_glob[index_sat]) # point_y (satellite elevation in skyplot)
area_error=math.fabs(IsInside(point1_x, point1_y, point2_x, point2_y, point0_x, point0_y, point_x, point_y))
# print 'area_error',area_error,'listx_2_ini_glob',listx_2_ini_glob
# print 'len(listx_2)=',len(listx_2),'len(listx_2_ini_glob)=',len(listx_2_ini_glob)
if(listx_2_ini_glob[i_bou_num]<0): # negtive to positive
listx_2_ini_glob[i_bou_num]=listx_2_ini_glob[i_bou_num]+360
if (listx_3_ini_glob[i_bou_num] < 0): # negtive to positive
listx_3_ini_glob[i_bou_num] = listx_3_ini_glob[i_bou_num] + 360
if(listx_2_ini_glob[i_bou_num]>listx_3_ini_glob[i_bou_num]) and len(listx_2_ini_glob)>0: # make sure that listx_3_initial[i_bou_num] is smaller
azi_temp=listx_3_ini_glob[i_bou_num]
listx_3_ini_glob[i_bou_num]=listx_2_ini_glob[i_bou_num]
listx_2_ini_glob[i_bou_num]=azi_temp
ang_ran_=float(listx_3_ini_glob[i_bou_num]-listx_2_ini_glob[i_bou_num])
print 'ang_ran_----------------------',ang_ran_,'from',listx_2_ini_glob[i_bou_num],'to',listx_3_ini_glob[i_bou_num]
# if(len(satellite_visability)>0): # if satellite_visability is not empty, clear
# del satellite_visability[:] # clear content of satellite_visability
if(ang_ran_<160):
if(satellite_initial_azimuth_list_glob[index_sat]>listx_2_ini_glob[i_bou_num]) and (satellite_initial_azimuth_list_glob[index_sat]<listx_3_ini_glob[i_bou_num]) and (area_error>5):# and (math.fabs(area_error)>3) :
print 'detect one NLOS signals at satellite-',satellite_number_list_glob[index_sat],'area_error',area_error,'satellite_initial_azimuth_list',satellite_initial_azimuth_list_glob[index_sat]
print 'point1_x point1_y point2_x point2_y point_x point_y',point1_x,point1_y,point2_x,point2_y,point_x,point_y
GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later
satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability
elif(ang_ran_>160):
if (satellite_initial_azimuth_list_glob[index_sat] > 0) and (satellite_initial_azimuth_list_glob[index_sat] < listx_2_ini_glob[i_bou_num]) and (area_error >5): # and (math.fabs(area_error)>3) :
print 'detect one NLOS signals at satellite-', satellite_number_list_glob[index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list',satellite_initial_azimuth_list_glob[index_sat]
print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y
GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later
satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability
if (satellite_initial_azimuth_list_glob[index_sat] < 360) and (satellite_initial_azimuth_list_glob[index_sat] > listx_3_ini_glob[i_bou_num]) and (area_error >5): # and (math.fabs(area_error)>3) :
print 'detect one NLOS signals at satellite-', satellite_number_list_glob[index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list',satellite_initial_azimuth_list_glob[index_sat]
print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y
GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later
satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability
GPSNumExcluded=0.0
BeidouNumExcluded=0.0
for del_index in range(len(GNSS_exclusion_Array_1.GNSS_Raws)): # index all the satellites' visability in one epoch
if (del_index >= len(GNSS_exclusion_Array_1.GNSS_Raws)):
break
if(GNSS_exclusion_Array_1.GNSS_Raws[del_index].visable == 2) or(GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation<=20): # if satellite is NLOS reception, delete this satellite
if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 1) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= 32)): # GPS
GPSNumExcluded=GPSNumExcluded+1 # one GPS exclusion
if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 88) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= (87+37))): # GPS
BeidouNumExcluded=BeidouNumExcluded+1 # one Beidou exclusion
del GNSS_exclusion_Array_1.GNSS_Raws[del_index] # delete the NLOS reception satellites
if(exclusMode==1): # mannual exclusion: specify certain amount of satellites needed to be excluded
for index_sat in range(len(GNSS_Raw_array_list)): # index all the satellite information in one epoch
if(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index in satellite_visability_mannually):# and (math.fabs(area_error)>3) :
GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later
if float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index) not in satellite_visability:
satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability
GPSNumExcluded=0.0
BeidouNumExcluded=0.0
for del_index in range(len(GNSS_exclusion_Array_1.GNSS_Raws)): # index all the satellites' visability in one epoch
if (del_index >= len(GNSS_exclusion_Array_1.GNSS_Raws)):
break
if(GNSS_exclusion_Array_1.GNSS_Raws[del_index].visable == 2) or(GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation<=20): # if satellite is NLOS reception, delete this satellite
if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 1) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= 32)): # GPS
GPSNumExcluded=GPSNumExcluded+1 # one GPS exclusion
if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 88) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= (87+37))): # GPS
BeidouNumExcluded=BeidouNumExcluded+1 # one Beidou exclusion
print 'GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation',GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation
del GNSS_exclusion_Array_1.GNSS_Raws[del_index] # delete the NLOS reception satellites
plot_GPS_sate_excluded.append(float(GPSNumExcluded)) # save GPS exclusion numbers
plot_Beidou_sate_excluded.append(float(BeidouNumExcluded)) # save Beidou exclusion numbers
plot_Total_sate_excluded.append(float(GPSNumExcluded)+float(BeidouNumExcluded)) # save all the satellites that have been excluded
plot_Total_sate_excludedTim.append(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))
satelliteInfo=Satellite_Info()
satelliteInfo.GNSS_time = int(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))
satelliteInfo.GPSExcluded = float(GPSNumExcluded)
satelliteInfo.BeidouExcluded = float(BeidouNumExcluded)
Satellite_Info_pub_.publish(satelliteInfo)
GPSNumExcluded = 0.0 # clear numbers
BeidouNumExcluded = 0.0 # clear numbers
if(len(listx_2_ini_glob)>0): # make sure, publish GNSS_exclusion_Array_1 only when there is at least one double-decker bus
GNSS_exclusion_pub.publish(GNSS_exclusion_Array_1)
del listx_2_ini_glob[:]
del listx_3_ini_glob[:]
del listx_2_glob[:]
del listy_2_glob[:]
del listx_3_glob[:]
del listy_3_glob[:]
del satellite_initial_azimuth_list_glob[:]
del satellite_azimuth_list_glob[:]
del satellite_elevation_list_glob[:]
del satellite_number_list_glob[:]
def readCSV(self):
self.csv_GNSS = csv.reader(open(
self.csvPath, 'r')) # read csv context to csv_reader variable
self.GNSSArr = GNSS_Raw_Array()
self.GNSSTim = 0
self.preGNSSTim = 0
self.linNum = 0
self.preTopiTim = time.time()
self.curTopTim = time.time()
for rowCsv in self.csv_GNSS:
self.preTopiTim = float(round(time.time() * 1000))
self.GNSS_ = GNSS_Raw()
self.rosNavSaFix = NavSatFix()
self.GNSSTim = rowCsv[0]
self.GNSS_.GNSS_time = float(
rowCsv[0]) # GNSS time contained in csv file
self.GNSS_.total_sv = float(
rowCsv[1]
) # total sitellites in one epoch (epoch: each time point)
self.GNSS_.prn_satellites_index = float(
rowCsv[2]
) # satellite index in this epoch (epoch: each time point)
self.GNSS_.pseudorange = float(
rowCsv[3]) - float(rowCsv[7]) - float(rowCsv[8]) + float(
rowCsv[9]) # pseudorange measurement
self.GNSS_.snr = float(
rowCsv[4]) # signal noise ratio for this satellite
self.GNSS_.elevation = float(
rowCsv[5]) # elevation for this satellite and receiver
self.GNSS_.azimuth = float(rowCsv[6]) # azimuth for this satellite
self.GNSS_.err_tropo = float(
rowCsv[7]) # troposphere error in meters
self.GNSS_.err_iono = float(rowCsv[8]) # ionophere error in meters
self.GNSS_.sat_clk_err = float(
rowCsv[9]) # satellite clock bias caused error
self.GNSS_.sat_pos_x = float(
rowCsv[10]) # satellite positioning in x direction
self.GNSS_.sat_pos_y = float(
rowCsv[11]) # satellite positioning in y direction
self.GNSS_.sat_pos_z = float(
rowCsv[12]) # satellite positioning in Z direction
self.GNSS_.visable = 0 # satellite visability juded by NLOS exclusion
if ((self.preGNSSTim == self.GNSSTim) or (self.linNum == 0)):
if (self.GNSS_.elevation > self.eleThres_) and (
self.GNSS_.snr >
self.snrThres_) and (self.GNSS_.prn_satellites_index
not in self.lowEleSatLis_):
self.GNSSArr.GNSS_Raws.append(self.GNSS_)
else:
self.GNSSArr.header.frame_id = 'GNSS_'
self.GNSS_Raw_pub.publish(self.GNSSArr)
# GNSSPosCal_ = puGNSSPosCal.GNSSPosCal()
# GNSSPosCal_.iterPosCal(self.GNSSArr,'LSGNSS')
# print 'llh',GNSSPosCal_.llh_,len(GNSSPosCal_.azimuth),GNSSPosCal_.azimuth
self.GNSSArr_ = self.GNSSArr # keep for outside
del self.GNSSArr.GNSS_Raws[:]
if (self.GNSS_.elevation > self.eleThres_) and (
self.GNSS_.snr >
self.snrThres_) and (self.GNSS_.prn_satellites_index
not in self.lowEleSatLis_):
self.GNSSArr.GNSS_Raws.append(self.GNSS_)
self.curTopTim = float(round(time.time() * 1000))
time.sleep(1 / self.topHz -
(self.curTopTim - self.preTopiTim) / 1000.0)
# time.sleep(1 / self.topHz )
print 'rostopic time bias / ms ------------ ', self.curTopTim - self.preTopiTim
self.preGNSSTim = self.GNSSTim
self.linNum = self.linNum + 1
def __init__(self, parent=None):
super(readCSV2TopiThread, self).__init__(parent)
self.GNSS_ = GNSS_Raw_Array()
def LSGPSPosCalStep(self, GNSS_one_epoch_, init_x, init_y, init_z,
init_b): # least square
GNSS_one_epoch = GNSS_Raw_Array()
GNSS_one_epoch = GNSS_one_epoch_
rec_ini_pos_x = float(init_x) # initial position x (m)
rec_ini_pos_y = float(init_y) # initial position y (m)
rec_ini_pos_z = float(init_z) # initial position z (m)
rec_clo_bia_b = float(
init_b) # initial distance bias caused by clock bias (m)
devia_xyz = 1.0 # initial set receiver position estimation error (m)
gues_pseud = [] # guessed pseudorange
pseud_error = [] # pseudorange error
G_matrix_ = array(
[2, 2, 2, 1],
dtype='float') # creat G matrix to save transform parameters
self.dop = self.DopCalculation(GNSS_one_epoch)
for index_1 in range(
len(GNSS_one_epoch.GNSS_Raws
)): # index all the satellite information in one epoch
if ((GNSS_one_epoch.GNSS_Raws[index_1].prn_satellites_index >= 88)
and
(GNSS_one_epoch.GNSS_Raws[index_1].prn_satellites_index <=
(87 + 37))):
if (self.iterations_ < 1): # save one time only
self.GNSSTim = GNSS_one_epoch.GNSS_Raws[index_1].GNSS_time
self.toSv = GNSS_one_epoch.GNSS_Raws[index_1].total_sv
self.prn.append(
GNSS_one_epoch.GNSS_Raws[index_1].prn_satellites_index)
self.snr.append(GNSS_one_epoch.GNSS_Raws[index_1].snr)
self.visi.append(GNSS_one_epoch.GNSS_Raws[index_1].visable)
self.azimuth.append(
GNSS_one_epoch.GNSS_Raws[index_1].azimuth)
self.elevation.append(
GNSS_one_epoch.GNSS_Raws[index_1].elevation)
sx_1 = float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_x
) # satellite position x
sy_1 = float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_y
) # satellite position y
sz_1 = float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_z
) # satellite position z
sx_1 = (sx_1 - rec_ini_pos_x) * (
sx_1 - rec_ini_pos_x
) # satellite to receiver distance in x idrection
sy_1 = (sy_1 - rec_ini_pos_y) * (
sy_1 - rec_ini_pos_y
) # satellite to receiver distance in y idrection
sz_1 = (sz_1 - rec_ini_pos_z) * (
sz_1 - rec_ini_pos_z
) # satellite to receiver distance in z idrection
sat2rec_dis = sqrt(sx_1 + sy_1 + sz_1) # guessed pseudorange
gues_pseud.append(sat2rec_dis) # save guessed pseudorange
pseud_error_element = float(
GNSS_one_epoch.GNSS_Raws[index_1].pseudorange) - float(
sat2rec_dis) + float(
rec_clo_bia_b) # pseudorange error
pseud_error.append(
pseud_error_element) # save pseudorange error
G_row = [] # G matrix row
G_row.append(
float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_x -
rec_ini_pos_x) / float(sat2rec_dis) *
-1) # x for G matrix row
G_row.append(
float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_y -
rec_ini_pos_y) / float(sat2rec_dis) *
-1) # y for G matrix row
G_row.append(
float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_z -
rec_ini_pos_z) / float(sat2rec_dis) *
-1) # z for G matrix row
element_float = 1.0 # last element for each row
G_row.append(element_float) # save last element for each row
G_matrix_ = np.row_stack(
(G_matrix_, G_row)) # add each row to G_matrix
del G_row[:] # relief G_row
# get pseudorange error
pseud_error_mat = np.array(pseud_error) # from list to array
pseud_error_mat = pseud_error_mat.transpose() # transpose
# get G matrix
G_matrix_ = np.delete(G_matrix_, [0],
axis=0) # delete the first row of G matrix
delta_p = np.dot((G_matrix_.transpose()), G_matrix_) # G(T) * G
delta_p_2 = np.linalg.inv(delta_p) # inverse matrix of G(T) * G
delta_p = np.dot(delta_p_2,
(G_matrix_.transpose()
)) # multiply (inverse matrix of G(T) * G) and G(T)
delta_p = np.dot(delta_p,
pseud_error_mat) # multiply with pseud_error_mat
rec_ini_pos_x = rec_ini_pos_x + float(
delta_p[0]) # update receiver position in x direction
rec_ini_pos_y = rec_ini_pos_y + float(
delta_p[1]) # update receiver position in y idrection
rec_ini_pos_z = rec_ini_pos_z + float(
delta_p[2]) # update receiver position in z idrection
rec_clo_bia_b = rec_clo_bia_b + float(
delta_p[3]) # update receiver clock bias in meters
devia_x = float(delta_p[0]) # save delta x
devia_y = float(delta_p[1]) # save delta y
devia_z = float(delta_p[2]) # save delta z
devia_b = float(delta_p[3]) # save delta bias
devia_xyz = sqrt(devia_x * devia_x + devia_y * devia_y +
devia_z * devia_z) # get total bias
# print 'delta_p',delta_p
# print 'position estimation x=',rec_ini_pos_x
# print 'position estimation y=', rec_ini_pos_y
# print 'position estimation Z=', rec_ini_pos_z
# print 'position estimation b=', rec_clo_bia_b
# print 'position estimation devia_xyz=', devia_xyz
del gues_pseud[:] # relief gues_pseud[] list
del pseud_error[:] # relief pseud_error[] list
return float(rec_ini_pos_x), float(rec_ini_pos_y), float(
rec_ini_pos_z), float(rec_clo_bia_b), float(devia_xyz)
def nlosExclusion_(self, dataPB, dataNG, mode, manuSatLis):
self.posArr = PoseArray()
self.IniGNSS_ = GNSS_Raw_Array()
self.nlosGNSS_ = GNSS_Raw_Array()
self.nlosGNSSDel_ = GNSS_Raw_Array()
self.mannuSat = manuSatLis
self.nlExcMode = mode
self.posArr = dataPB
self.IniGNSS_ = dataNG
self.nlosGNSS_ = dataNG
self.lateralDis = 5.8 # for Transaction on Vehicle Technology
self.GNSSCovariance = 0
self.getSatNum(self.IniGNSS_)
lenPosArr_ = 0.0
lenPosArr_ = len(
self.posArr) # print 'len(self.posArr)------',len(self.posArr)
if (lenPosArr_ > 2):
lenPosArr_ = 2
for i in range(lenPosArr_):
# x--azimuth y--elevation
self.bouSide1X.append(
1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) *
np.cos((self.posArr[i].orientation.x - 90.0 + self.calAngN) *
3.14159 / 180.0)) # start point azimuth
self.bouSide1Y.append(
1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) *
np.sin((self.posArr[i].orientation.x - 90.0 + self.calAngN) *
3.14159 / 180.0)) # start point elevation
azi_temp = 0.0
azi_temp = (math.atan2(self.bouSide1Y[-1],
self.bouSide1X[-1])) * 180.0 / (math.pi)
if (azi_temp < 0.0):
azi_temp = azi_temp + 360
self.bouSide1IniX.append(
float(azi_temp)) # initial azimuth start point
# print 'start point azimuth IN PUNLOSEXCLUSION===', azi_temp,self.bouSide1IniX[-1],len(self.bouSide1IniX)
self.bouSide2X.append(
1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) *
np.cos((self.posArr[i].orientation.z - 90.0 + self.calAngN) *
3.14159 / 180.0)) # end point azimuth
self.bouSide2Y.append(
1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) *
np.sin((self.posArr[i].orientation.z - 90.0 + self.calAngN) *
3.14159 / 180.0)) # end point elevation
azi_temp = (math.atan2(self.bouSide2Y[-1],
self.bouSide2X[-1])) * 180.0 / (math.pi)
if (azi_temp < 0.0):
azi_temp = azi_temp + 360
self.bouSide2IniX.append(
float(azi_temp)) # initial azimuth start point
# print 'start point azimuth===', azi_temp
for index_1 in range(
len(self.IniGNSS_.GNSS_Raws
)): # index all the satellite information in one epoch
self.azim_.append(
(self.IniGNSS_.GNSS_Raws[index_1].elevation *
(-0.55556) + 50.0) *
np.cos(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) *
3.14159 / 180.0))
self.elev_.append(
(self.IniGNSS_.GNSS_Raws[index_1].elevation *
(-0.55556) + 50.0) *
np.sin(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) *
3.14159 / 180.0))
sateazi_ = (
360 - (self.IniGNSS_.GNSS_Raws[index_1].azimuth)
) + 90 # change azimuth value into the coordinate that the same with the bus one (bouSide2X)
if (sateazi_ > 360):
sateazi_ = sateazi_ - 360
self.azimIni_.append(sateazi_)
self.elevIni_.append(self.IniGNSS_.GNSS_Raws[index_1].elevation)
self.satIdx.append(
self.IniGNSS_.GNSS_Raws[index_1].prn_satellites_index)
self.GNSSTime = self.IniGNSS_.GNSS_Raws[index_1].GNSS_time
if (self.nlExcMode == 'statAuto'):
for idx in range(
len(self.bouSide1X)
): # index all the boundary line (usually only one double-decker bus, sometime 2)
for index_sat in range(
len(self.IniGNSS_.GNSS_Raws)
): # index all the satellite information in one epoch
point0_x = 0.0 # yuanxin x
point0_y = 0.0 # yuanxin y
point1_x = float(
self.bouSide1X[idx]) # point1 coordinate x
point1_y = float(
self.bouSide1Y[idx]) # point1 coordinate y
point2_x = float(
self.bouSide2X[idx]) # point2 coordinate x
point2_y = float(
self.bouSide2Y[idx]) # point2 coordinate y
point_x = float(self.azim_[index_sat]
) # point_x (satellite azimuth in skyplot)
point_y = float(
self.elev_[index_sat]
) # point_y (satellite elevation in skyplot)
self.DistanceThres_ = 10
area_error = math.fabs(
self.IsInside(point1_x, point1_y, point2_x, point2_y,
point0_x, point0_y, point_x, point_y))
if (self.bouSide1IniX[idx] >
self.bouSide2IniX[idx]) and len(
self.bouSide2IniX
) > 0: # make sure that bouSide1IniX is smaller
azi_temp_ = self.bouSide2IniX[idx]
self.bouSide2IniX[idx] = self.bouSide1IniX[idx]
self.bouSide1IniX[idx] = azi_temp_
ang_ran_ = float(self.bouSide2IniX[idx] -
self.bouSide1IniX[idx])
# print 'ang_ran_----------------------', ang_ran_, 'from', self.bouSide1IniX[idx], 'to', self.bouSide2IniX[idx]
angDif1 = 0.0
angDif2 = 0.0
if (ang_ran_ < self.bouRang):
angDif1 = float(
math.fabs(self.azimIni_[index_sat] -
self.bouSide1IniX[idx]))
angDif2 = float(
math.fabs(self.azimIni_[index_sat] -
self.bouSide2IniX[idx]))
# print 'angDif1= ',angDif1,'angDif2= ',angDif2
if (self.azimIni_[index_sat] > self.bouSide1IniX[idx]
) and (self.azimIni_[index_sat] <
self.bouSide2IniX[idx]) and (
area_error > self.DistanceThres_) and (
angDif1 > 3) and (angDif2 > 3): #
self.nlosGNSS_.GNSS_Raws[
index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
elif (ang_ran_ > self.bouRang):
if (self.azimIni_[index_sat] >
0) and (self.azimIni_[index_sat] <
self.bouSide1IniX[idx]) and (
area_error > self.DistanceThres_
): # and (math.fabs(area_error)>3) :
self.nlosGNSS_.GNSS_Raws[
index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
if (self.azimIni_[index_sat] <
360) and (self.azimIni_[index_sat] >
self.bouSide2IniX[idx]) and (
area_error > self.DistanceThres_
): # and (math.fabs(area_error)>3) :
# print 'detect one NLOS signals at satellite-', self.satIdx[
# index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list', self.azimIni_[index_sat]
# print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y
self.nlosGNSS_.GNSS_Raws[
index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
self.GNSSNlos_pub.publish(self.nlosGNSS_)
exclusionSatNum_ = exclusionSatNum()
exclusionSatNum_.satlist = self.satExcl_
self.exclusionSatnum_Pub.publish(exclusionSatNum_)
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
if (self.nlosGNSS_.GNSS_Raws[saIdx].visable !=
2): # if satellite is none Light of sight (save)
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
self.getExclSatNum()
if (self.nlExcMode == 'statManu_Correction'):
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
if ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index
in self.mannuSat)
and (len(self.nlosGNSS_.GNSS_Raws) >
9)): # if satellite is none Light of sight (save)
# GNSS_Raw_Correction = GNSS_Raw()
# afa = 4.0
# GNSS_Raw_Correction = self.nlosGNSS_.GNSS_Raws[saIdx]
# kama1 = afa * ((1 / (math.cos(GNSS_Raw_Correction.elevation * math.pi / 180.0))) * (1 + math.cos(2 * GNSS_Raw_Correction.elevation * math.pi / 180.0)))
# kama2 = afa * ((1 / (math.cos(GNSS_Raw_Correction.azimuth * math.pi / 180.0))) * (1 + math.cos(2 * GNSS_Raw_Correction.azimuth * math.pi / 180.0)))
# kama = float(math.fabs(kama1) + math.fabs(kama2) )
# # GNSS_Raw_Correction.pseudorange = GNSS_Raw_Correction.pseudorange - kama
# print 'kama1----', type(kama1),' kama2----', kama2,' kama----', kama, ' pseudorange----', GNSS_Raw_Correction.pseudorange
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
ele = self.nlosGNSSDel_.GNSS_Raws[
-1].elevation * 3.14 / 180.0
azi = self.nlosGNSSDel_.GNSS_Raws[-1].azimuth * 3.14 / 180.0
cor_1 = self.lateralDis * cos(float(ele))
cor_1 = math.fabs(cor_1 * (1.0 + cos(2.0 * ele)))
cor_2 = self.lateralDis * cos(float(azi))
cor_2 = math.fabs(cor_2 * (1.0 + cos(2.0 * azi)))
print 'cor_1 ', cor_1, 'cor_2 ', cor_2
self.nlosGNSSDel_.GNSS_Raws[
-1].pseudorange = self.nlosGNSSDel_.GNSS_Raws[
-1].pseudorange - (cor_1 + cor_2)
# if( (self.nlosGNSSDel_.GNSS_Raws[-1].elevation >=54 ) and (self.nlosGNSSDel_.GNSS_Raws[-1].elevation <= 72)):
# self.nlosGNSSDel_.GNSS_Raws[-1].pseudorange = self.nlosGNSSDel_.GNSS_Raws[-1].pseudorange - (cor_1 + cor_2)
# print 'self.nlosGNSSDel_.GNSS_Raws[-1].prn_satellites_index',self.nlosGNSSDel_.GNSS_Raws[-1].prn_satellites_index
elif ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index
not in self.mannuSat)
or (len(self.nlosGNSS_.GNSS_Raws) <= 9)): #no correction
# self.nlosGNSS_.GNSS_Raws[saIdx].visable =2
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
self.satExcl_ = self.mannuSat
self.GNSSNlos_pub.publish(self.nlosGNSS_)
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
exclusionSatNum_ = exclusionSatNum()
exclusionSatNum_.satlist = self.satExcl_
self.exclusionSatnum_Pub.publish(exclusionSatNum_)
self.getExclSatNum()
if (self.nlExcMode == 'statManu_ExclusionClosedLoop'):
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
dangerArea = 1
# if danger area, exclusion is needed
if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 176810000) and
(self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 176834000)):
# print 'safe area '
dangerArea = 0
if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 176875000) and
(self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 176908000)):
dangerArea = 0
# print 'safe area '
# corner start: 176810000
#corner end: 176834000.0
# corner start: 176875000
# corner end: 176908000
cor_1 = 0
cor_2 = 0
# this satellites should be exlude if satellite is NLOS and the elevation angle is low
if ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index
in self.mannuSat)
and (dangerArea
== 1)): # if satellite is Light of sight (save)
print 'cor_1 ', cor_1, 'cor_2 ', cor_2, 'GNSS time --', self.nlosGNSS_.GNSS_Raws[
-1].GNSS_time
elif ((dangerArea == 0)): #no exclusion
# self.nlosGNSS_.GNSS_Raws[saIdx].visable =2
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
self.GNSSCovariance = 4.3 + self.GNSSCovariance
elif ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index
not in self.mannuSat)
and (dangerArea == 1)): #no exclusion
# self.nlosGNSS_.GNSS_Raws[saIdx].visable =2
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
ele = self.nlosGNSS_.GNSS_Raws[
saIdx].elevation * 3.14 / 180.0
azi = self.nlosGNSS_.GNSS_Raws[saIdx].azimuth * 3.14 / 180.0
cor_1 = self.lateralDis * cos(float(ele))
cor_1 = math.fabs(cor_1 * (1.0 + cos(2.0 * ele)))
cor_2 = self.lateralDis * cos(float(azi))
cor_2 = math.fabs(cor_2 * (1.0 + cos(2.0 * azi)))
self.GNSSCovariance = self.GNSSCovariance + (cor_1 + cor_2)
self.satExcl_ = self.mannuSat
self.GNSSNlos_pub.publish(self.nlosGNSS_)
self.GNSSCovariance = self.GNSSCovariance + 4.5
self.GNSSCovariance = self.GNSSCovariance * 0.6
print 'self.GNSSCovariance== ', self.GNSSCovariance
self.nlosGNSSDel_.GNSS_Raws[-1].snr = self.GNSSCovariance
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
exclusionSatNum_ = exclusionSatNum()
exclusionSatNum_.satlist = self.satExcl_
self.exclusionSatnum_Pub.publish(exclusionSatNum_)
self.getExclSatNum()
if (self.nlExcMode == 'statManu_ExclusionOpenLoop'):
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
dangerArea = 1
# if danger area, exclusion is needed
if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 47745000) and
(self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 47960000)):
# print 'safe area '
self.mannuSat = [5, 24, 89, 13, 2,
88] #5,24 5,24,89,13,2,88
if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 47960000) and
(self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 47990000)):
self.mannuSat = [5, 90, 29, 15, 95, 87, 2,
88] # 5,90,29 5,90,29,15,95,87,2,88
# print 'safe area '
if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 47990000) and
(self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 48114000)):
self.mannuSat = [24, 2, 87, 5, 88, 94, 90,
89] # 2,24,5 24,2,87,5,88,94,90,89
# print 'safe area '
# corner start: 176810000
#corner end: 176834000.0
# corner start: 176875000
# corner end: 176908000
cor_1 = 0
cor_2 = 0
# this satellites should be exlude if satellite is NLOS and the elevation angle is low
if ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index
in self.mannuSat)
and (dangerArea
== 1)): # if satellite is Light of sight (save)
print 'cor_1 ', cor_1, 'cor_2 ', cor_2, 'GNSS time --', self.nlosGNSS_.GNSS_Raws[
-1].GNSS_time
elif ((dangerArea == 0)): #no exclusion
# self.nlosGNSS_.GNSS_Raws[saIdx].visable =2
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
self.GNSSCovariance = 4.3 + self.GNSSCovariance
elif ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index
not in self.mannuSat)
and (dangerArea == 1)): #no exclusion
# self.nlosGNSS_.GNSS_Raws[saIdx].visable =2
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
ele = self.nlosGNSS_.GNSS_Raws[
saIdx].elevation * 3.14 / 180.0
azi = self.nlosGNSS_.GNSS_Raws[saIdx].azimuth * 3.14 / 180.0
cor_1 = self.lateralDis * cos(float(ele))
cor_1 = math.fabs(cor_1 * (1.0 + cos(2.0 * ele)))
cor_2 = self.lateralDis * cos(float(azi))
cor_2 = math.fabs(cor_2 * (1.0 + cos(2.0 * azi)))
self.GNSSCovariance = self.GNSSCovariance + (cor_1 + cor_2)
self.satExcl_ = self.mannuSat
self.GNSSNlos_pub.publish(self.nlosGNSS_)
self.GNSSCovariance = self.GNSSCovariance + 4.5
self.GNSSCovariance = self.GNSSCovariance * 0.6
print 'self.GNSSCovariance== ', self.GNSSCovariance
self.nlosGNSSDel_.GNSS_Raws[-1].snr = self.GNSSCovariance
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
exclusionSatNum_ = exclusionSatNum()
exclusionSatNum_.satlist = self.satExcl_
self.exclusionSatnum_Pub.publish(exclusionSatNum_)
self.getExclSatNum()
def CallGNSSRAWNlosDel_(self, data): # subscribe data after exclusion
self.GNSSNRAWlosDel = GNSS_Raw_Array()
self.GNSSNRAWlosDel = data
def correctNLOS(self, data):
correctedGNSS_ = GNSS_Raw_Array()
def nlosExclusion_(self, dataPB, dataNG, mode, manuSatLis):
self.posArr = PoseArray()
self.IniGNSS_ = GNSS_Raw_Array()
self.nlosGNSS_ = GNSS_Raw_Array()
self.nlosGNSSDel_ = GNSS_Raw_Array()
self.mannuSat = manuSatLis
self.nlExcMode = mode
self.posArr = dataPB
self.IniGNSS_ = dataNG
self.nlosGNSS_ = dataNG
self.getSatNum(self.IniGNSS_)
lenPosArr_ = 0.0
lenPosArr_ = len(
self.posArr) # print 'len(self.posArr)------',len(self.posArr)
if (lenPosArr_ > 2):
lenPosArr_ = 2
for i in range(lenPosArr_):
# x--azimuth y--elevation
self.bouSide1X.append(
1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) *
np.cos((self.posArr[i].orientation.x - 90.0 + self.calAngN) *
3.14159 / 180.0)) # start point azimuth
self.bouSide1Y.append(
1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) *
np.sin((self.posArr[i].orientation.x - 90.0 + self.calAngN) *
3.14159 / 180.0)) # start point elevation
azi_temp = 0.0
azi_temp = (math.atan2(self.bouSide1Y[-1],
self.bouSide1X[-1])) * 180.0 / (math.pi)
if (azi_temp < 0.0):
azi_temp = azi_temp + 360
self.bouSide1IniX.append(
float(azi_temp)) # initial azimuth start point
# print 'start point azimuth IN PUNLOSEXCLUSION===', azi_temp,self.bouSide1IniX[-1],len(self.bouSide1IniX)
self.bouSide2X.append(
1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) *
np.cos((self.posArr[i].orientation.z - 90.0 + self.calAngN) *
3.14159 / 180.0)) # end point azimuth
self.bouSide2Y.append(
1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) *
np.sin((self.posArr[i].orientation.z - 90.0 + self.calAngN) *
3.14159 / 180.0)) # end point elevation
azi_temp = (math.atan2(self.bouSide2Y[-1],
self.bouSide2X[-1])) * 180.0 / (math.pi)
if (azi_temp < 0.0):
azi_temp = azi_temp + 360
self.bouSide2IniX.append(
float(azi_temp)) # initial azimuth start point
# print 'start point azimuth===', azi_temp
for index_1 in range(
len(self.IniGNSS_.GNSS_Raws
)): # index all the satellite information in one epoch
self.azim_.append(
(self.IniGNSS_.GNSS_Raws[index_1].elevation *
(-0.55556) + 50.0) *
np.cos(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) *
3.14159 / 180.0))
self.elev_.append(
(self.IniGNSS_.GNSS_Raws[index_1].elevation *
(-0.55556) + 50.0) *
np.sin(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) *
3.14159 / 180.0))
sateazi_ = (
360 - (self.IniGNSS_.GNSS_Raws[index_1].azimuth)
) + 90 # change azimuth value into the coordinate that the same with the bus one (bouSide2X)
if (sateazi_ > 360):
sateazi_ = sateazi_ - 360
self.azimIni_.append(sateazi_)
self.elevIni_.append(self.IniGNSS_.GNSS_Raws[index_1].elevation)
self.satIdx.append(
self.IniGNSS_.GNSS_Raws[index_1].prn_satellites_index)
self.GNSSTime = self.IniGNSS_.GNSS_Raws[index_1].GNSS_time
if (self.nlExcMode == 'statAuto'):
for idx in range(
len(self.bouSide1X)
): # index all the boundary line (usually only one double-decker bus, sometime 2)
for index_sat in range(
len(self.IniGNSS_.GNSS_Raws)
): # index all the satellite information in one epoch
point0_x = 0.0 # yuanxin x
point0_y = 0.0 # yuanxin y
point1_x = float(
self.bouSide1X[idx]) # point1 coordinate x
point1_y = float(
self.bouSide1Y[idx]) # point1 coordinate y
point2_x = float(
self.bouSide2X[idx]) # point2 coordinate x
point2_y = float(
self.bouSide2Y[idx]) # point2 coordinate y
point_x = float(self.azim_[index_sat]
) # point_x (satellite azimuth in skyplot)
point_y = float(
self.elev_[index_sat]
) # point_y (satellite elevation in skyplot)
self.DistanceThres_ = 10
area_error = math.fabs(
self.IsInside(point1_x, point1_y, point2_x, point2_y,
point0_x, point0_y, point_x, point_y))
if (self.bouSide1IniX[idx] >
self.bouSide2IniX[idx]) and len(
self.bouSide2IniX
) > 0: # make sure that bouSide1IniX is smaller
azi_temp_ = self.bouSide2IniX[idx]
self.bouSide2IniX[idx] = self.bouSide1IniX[idx]
self.bouSide1IniX[idx] = azi_temp_
ang_ran_ = float(self.bouSide2IniX[idx] -
self.bouSide1IniX[idx])
# print 'ang_ran_----------------------', ang_ran_, 'from', self.bouSide1IniX[idx], 'to', self.bouSide2IniX[idx]
angDif1 = 0.0
angDif2 = 0.0
if (ang_ran_ < self.bouRang):
angDif1 = float(
math.fabs(self.azimIni_[index_sat] -
self.bouSide1IniX[idx]))
angDif2 = float(
math.fabs(self.azimIni_[index_sat] -
self.bouSide2IniX[idx]))
# print 'angDif1= ',angDif1,'angDif2= ',angDif2
if (self.azimIni_[index_sat] > self.bouSide1IniX[idx]
) and (self.azimIni_[index_sat] <
self.bouSide2IniX[idx]) and (
area_error > self.DistanceThres_) and (
angDif1 > 3) and (angDif2 > 3): #
self.nlosGNSS_.GNSS_Raws[
index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
elif (ang_ran_ > self.bouRang):
if (self.azimIni_[index_sat] >
0) and (self.azimIni_[index_sat] <
self.bouSide1IniX[idx]) and (
area_error > self.DistanceThres_
): # and (math.fabs(area_error)>3) :
self.nlosGNSS_.GNSS_Raws[
index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
if (self.azimIni_[index_sat] <
360) and (self.azimIni_[index_sat] >
self.bouSide2IniX[idx]) and (
area_error > self.DistanceThres_
): # and (math.fabs(area_error)>3) :
# print 'detect one NLOS signals at satellite-', self.satIdx[
# index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list', self.azimIni_[index_sat]
# print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y
self.nlosGNSS_.GNSS_Raws[
index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
self.GNSSNlos_pub.publish(self.nlosGNSS_)
exclusionSatNum_ = exclusionSatNum()
exclusionSatNum_.satlist = self.satExcl_
self.exclusionSatnum_Pub.publish(exclusionSatNum_)
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
if (self.nlosGNSS_.GNSS_Raws[saIdx].visable !=
2): # if satellite is Light of sight (save)
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
self.getExclSatNum()
if (self.nlExcMode == 'statManu'):
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
if (self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index
not in self.mannuSat
): # if satellite is Light of sight (save)
self.nlosGNSSDel_.GNSS_Raws.append(
self.nlosGNSS_.GNSS_Raws[saIdx])
elif (self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index in
self.mannuSat): # if satellite is Light of sight (save)
self.nlosGNSS_.GNSS_Raws[saIdx].visable = 2
self.satExcl_ = self.mannuSat
self.GNSSNlos_pub.publish(self.nlosGNSS_)
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
exclusionSatNum_ = exclusionSatNum()
exclusionSatNum_.satlist = self.satExcl_
self.exclusionSatnum_Pub.publish(exclusionSatNum_)
self.getExclSatNum()
def __init__(self, parent=None):
QMainWindow.__init__(self, parent)
self.setWindowTitle('GNSS Evaluation')
self.create_main_frame()
self.bestPos_ = BESTPOS()
self.navfixTruth_ = NavSatFix()
self.navfix_ = NavSatFix()
self.navfixprop_ = NavSatFix()
self.GNSS_ = GNSS_Raw_Array()
self.GNSSNRAWlosDel = GNSS_Raw_Array()
self.SatNum = []
self.HDOP = []
self.error = []
self.errorEv = [] # error for further eveluation
self.busAvail = [] # actual bus availibility
self.GNSSTime = []
self.SatNumProp = []
self.HDOPProp = []
self.errorProp = []
self.errorPropEv = [] # error for further eveluation
self.busAvailProp = [
] # proposed bus availibility which is subjected to the performance of object detection
self.GNSSTimeProp = []
self.lat_ = []
self.lon_ = []
self.latProp_ = []
self.lonProp_ = []
self.firstTime_ = 0.0
self.firstTimeProp_ = 0.0
self.preTime = 0.0
self.checkTimes = 0.0
self.finishSaving = 0.0
self.calAngNNLOS = 0.0
self.navfixAvailable = 0
self.navfixpropAvailable = 0
self.timer = QtCore.QTimer()
QtCore.QObject.connect(self.timer, QtCore.SIGNAL("timeout()"),
self.checkEnd)
self.timer.start(3000)
rospy.Subscriber('/novatel_data/bestpos', BESTPOS,
self.callcptBestPos_) # Ground truth
rospy.Subscriber('GNSS_', GNSS_Raw_Array,
self.CallGNSS_) # Ublox Raw data
rospy.Subscriber('novatel_data/inspvax', INSPVAX,
self.callINSPVAXNLOS_) # yaw
rospy.Subscriber(
'GNSS_NavFix', NavSatFix,
self.GNSSNavCall) # conventional GNSS positioning result
rospy.Subscriber(
'GNSS_NavFix_pro', NavSatFix,
self.GNSSNavPropCall) # proposed GNSS positioning result
rospy.Subscriber('GNSSRAWNlosDel_', GNSS_Raw_Array,
self.CallGNSSRAWNlosDel_1) # Ublox
rospy.Subscriber('velodyne_points_0', PointCloud2,
self.PointCloudCall) # velodyne
self.DOP_Pub = rospy.Publisher('DOP_', DOP, queue_size=100) #
self.Error_Pub = rospy.Publisher('ErrorGNSSXYZ_',
Error,
queue_size=100) #
self.DOPProp_Pub = rospy.Publisher('DOPProp_', DOP, queue_size=100) #
self.ErrorProp_Pub = rospy.Publisher('ErrorGNSSXYZProp_',
Error,
queue_size=100) #
def CallGNSS_(self, data): # GNSS data # 1Hz
self.GNSS_ = GNSS_Raw_Array()
self.GNSS_ = data
def nlosExclusion_(self, dataPB, dataNG, mode, manuSatLis):
self.posArr = PoseArray()
self.IniGNSS_ = GNSS_Raw_Array()
self.nlosGNSS_ = GNSS_Raw_Array()
self.nlosGNSSDel_ = GNSS_Raw_Array()
self.mannuSat= manuSatLis
self.nlExcMode = mode
self.posArr = dataPB
self.IniGNSS_ = dataNG
self.nlosGNSS_ = dataNG
self.getSatNum(self.IniGNSS_)
lenPosArr_ = 0.0
lenPosArr_ = len(self.posArr)
print 'len(self.posArr)------',len(self.posArr)
if(lenPosArr_ > 2):
lenPosArr_ = 2
for i in range(lenPosArr_):
# x--azimuth y--elevation
azi_temp = float(self.posArr [i].orientation.x) # angle direction
print 'start point azimuth===', azi_temp, 'self.calAngN', self.calAngN
if (azi_temp < 0.0):
azi_temp = azi_temp + 360
if (azi_temp > 360.0):
azi_temp = azi_temp - 360.0
self.bouSide1IniX.append(float(azi_temp)) # initial azimuth start point
azi_temp = float(self.posArr [i].orientation.z) # angle direction is reverse in skyplot and common coordinate
print 'end point azimuth===', azi_temp, 'self.calAngN', self.calAngN
if (azi_temp < 0):
azi_temp = azi_temp + 360.0
if(azi_temp > 360.0):
azi_temp = azi_temp -360.0
self.bouSide2IniX.append(azi_temp) # initial azimuth end point
self.bouSide1X.append(1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point azimuth
self.bouSide1Y.append(1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point elevation
self.bouSide2X.append(1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point azimuth
self.bouSide2Y.append(1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point elevation
for index_1 in range(len(self.IniGNSS_.GNSS_Raws)): # index all the satellite information in one epoch
self.azim_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.cos(-1*(self.IniGNSS_.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0))
self.elev_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.sin(-1*(self.IniGNSS_.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0))
self.azimIni_.append(self.IniGNSS_.GNSS_Raws[index_1].azimuth)
self.elevIni_.append(self.IniGNSS_.GNSS_Raws[index_1].elevation)
self.satIdx.append(self.IniGNSS_.GNSS_Raws[index_1].prn_satellites_index)
self.GNSSTime = self.IniGNSS_.GNSS_Raws[index_1].GNSS_time
if(self.nlExcMode=='statAuto'):
for idx in range(len(self.bouSide1X)): # index all the boundary line (usually only one double-decker bus, sometime 2)
for index_sat in range(len(self.IniGNSS_.GNSS_Raws)): # index all the satellite information in one epoch
point0_x = 0.0 # yuanxin x
point0_y = 0.0 # yuanxin y
point1_x = float(self.bouSide1X[idx]) # point1 coordinate x
point1_y = float(self.bouSide1Y[idx]) # point1 coordinate y
point2_x = float(self.bouSide2X[idx]) # point2 coordinate x
point2_y = float(self.bouSide2Y[idx]) # point2 coordinate y
point_x = float(self.azim_[index_sat]) # point_x (satellite azimuth in skyplot)
point_y = float(self.elev_[index_sat]) # point_y (satellite elevation in skyplot)
area_error = math.fabs(
self.IsInside(point1_x, point1_y, point2_x, point2_y, point0_x, point0_y, point_x, point_y))
if (self.bouSide1IniX[idx] < 0):
self.bouSide1IniX[idx] = self.bouSide1IniX[idx] + 360
if(self.bouSide2IniX[idx]<0):
self.bouSide2IniX[idx] = self.bouSide2IniX[idx] + 360
if (self.bouSide1IniX[idx] > self.bouSide2IniX[idx]) and len(
self.bouSide2IniX) > 0: # make sure that listx_3_initial[i_bou_num] is smaller
azi_temp = self.bouSide2IniX[idx]
self.bouSide2IniX[idx] = self.bouSide1IniX[idx]
self.bouSide1IniX[idx] = azi_temp
ang_ran_ = float(self.bouSide2IniX[idx] - self.bouSide1IniX[idx])
# print 'ang_ran_----------------------', ang_ran_, 'from', self.bouSide1IniX[idx], 'to', self.bouSide2IniX[idx]
if (ang_ran_ < self.bouRang):
if (self.azimIni_[index_sat] > self.bouSide1IniX[idx]) and (
self.azimIni_[index_sat] < self.bouSide2IniX[idx]) and (
area_error > 5): #
self.nlosGNSS_.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
elif (ang_ran_ > self.bouRang):
if (self.azimIni_[index_sat] > 0) and (self.azimIni_[index_sat] < self.bouSide1IniX[idx]) and (area_error > 5): # and (math.fabs(area_error)>3) :
self.nlosGNSS_.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
if (self.azimIni_[index_sat] < 360) and (
self.azimIni_[index_sat] > self.bouSide2IniX[idx]) and (
area_error > 5): # and (math.fabs(area_error)>3) :
# print 'detect one NLOS signals at satellite-', self.satIdx[
# index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list', self.azimIni_[index_sat]
# print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y
self.nlosGNSS_.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later
self.satExcl_.append(self.satIdx[index_sat])
self.GNSSNlos_pub.publish(self.nlosGNSS_)
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
if (self.nlosGNSS_.GNSS_Raws[saIdx].visable != 2): # if satellite is Light of sight (save)
self.nlosGNSSDel_.GNSS_Raws.append(self.nlosGNSS_.GNSS_Raws[saIdx])
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
self.getExclSatNum()
if (self.nlExcMode == 'statManu'):
for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)):
if (self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index not in self.mannuSat): # if satellite is Light of sight (save)
self.nlosGNSSDel_.GNSS_Raws.append(self.nlosGNSS_.GNSS_Raws[saIdx])
elif(self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index in self.mannuSat): # if satellite is Light of sight (save)
self.nlosGNSS_.GNSS_Raws[saIdx].visable =2
self.satExcl_ = self.mannuSat
self.GNSSNlos_pub.publish(self.nlosGNSS_)
self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_)
self.getExclSatNum()
print 'self.satExcl_=',self.satExcl_,len(self.nlosGNSSDel_.GNSS_Raws),len(self.nlosGNSS_.GNSS_Raws),self.nlosGNSS_.GNSS_Raws[0].total_sv
def CallGNSSRAWNlosDel_1(self,data):
self.GNSSNRAWlosDel = GNSS_Raw_Array()
self.GNSSNRAWlosDel = data