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 callcptBestPos_llh(self,data):
self.bestPos_ = BESTPOS()
self.bestPos_ = data
self.lat_.append(float(self.bestPos_.latitude))
self.lon_.append(float(self.bestPos_.longitude))
self.GPS_Week_Second = self.bestPos_.header.gps_week_seconds
print 'GPS_Week_Second',self.GPS_Week_Second
def callcptBestPos_(self,data): # 1Hz
self.bestPos_ = BESTPOS()
self.bestPos_ = data
self.navfixTruth_ = NavSatFix()
self.navfixTruth_.header.stamp.secs = float(data.header.gps_week_seconds)
self.navfixTruth_.latitude = float(data.latitude)
self.navfixTruth_.longitude = float(data.longitude)
self.navfixTruth_.altitude = float(data.altitude)
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.Covariance = []
self.CovarianceFixed = []
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 callcptBestPos(self,data):
self.bestPos_ = BESTPOS()
self.bestPos_ = data
self.Odometry_ = Odometry()
self.Odometry_.header.frame_id = 'odom'
self.Odometry_.header.seq = self.bestPos_.header.gps_week_seconds
self.Odometry_.header.stamp = self.Imu_.header.stamp
self.Odometry_.child_frame_id = 'base_link'
self.transENU_ = ()
self.lat_ = self.bestPos_.latitude
self.lon_ = self.bestPos_.longitude
self.alt_ = self.bestPos_.altitude
if(self.oriLat_ == 0):
self.oriLat_ = self.lat_
self.oriLon_ = self.lon_
self.oriAlt_ = self.alt_
self.originLonLatAlt_.append(self.lon_)
self.originLonLatAlt_.append(self.lat_)
self.originLonLatAlt_.append(self.alt_)
self.currentLonLatAlt_.append(self.lon_)
self.currentLonLatAlt_.append(self.lat_)
self.currentLonLatAlt_.append(self.alt_)
# print 'origin ',self.originLonLatAlt_
# print 'current',self.currentLonLatAlt_
self.ENU_= transformLonLatAltToEnu(tuple(self.originLonLatAlt_),tuple(self.currentLonLatAlt_))
self.transENU_ = self.transferENU(self.ENU_)
print 'self.transENU_=',self.transENU_
print 'ENU_', self.ENU_, math.sqrt(
self.ENU_[0] * self.ENU_[0] + self.ENU_[1] * self.ENU_[1] + self.ENU_[2] * self.ENU_[2])
self.Odometry_.pose.pose.position.x = self.transENU_[0]
self.Odometry_.pose.pose.position.y = self.transENU_[1]
self.Odometry_.pose.pose.position.z = self.transENU_[2] #self.alt_
Quaternion_ = (self.Imu_.orientation.x,self.Imu_.orientation.y,self.Imu_.orientation.z,self.Imu_.orientation.w)
euler = tf.transformations.euler_from_quaternion(Quaternion_)
if(self.firsYaw == 0):
self.firsYaw = euler[2]
print 'self.firsYaw ',self.firsYaw*180/pi
print 'euler----',(euler[2]-self.firsYaw)*180/pi
q_orig = quaternion_from_euler(euler[0], euler[1], (euler[2]-self.firsYaw))
q_rot = quaternion_from_euler(0, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)
eulerNew = tf.transformations.euler_from_quaternion(q_new)
print 'euler New ----', eulerNew[2]*180/pi
# print type(Quaternion_),Quaternion_,type(q_new),q_new
self.Odometry_.pose.pose.orientation.x = q_new[0]
self.Odometry_.pose.pose.orientation.y = q_new[1]
self.Odometry_.pose.pose.orientation.z = q_new[2]
self.Odometry_.pose.pose.orientation.w = q_new[3]
self.OdometryPub_.publish(self.Odometry_)
self.calRelativePosition()
print '--------------------------------'
self.currentLonLatAlt_[:]=[]
def callcptLLh(data):
llhCpt_ = BESTPOS()
llhCpt_ = data
gps_week.append(llhCpt_.header.gps_week)
gps_second.append(llhCpt_.header.gps_week_seconds)
list_lat.append(llhCpt_.latitude)
list_lon.append(float(llhCpt_.longitude))
altitude.append(float(llhCpt_.altitude))
if (len(list_lon) > 18):
with open(
filename, 'w'
) as file_object: # save listLatRaw and listLonRaw to csv file
for sav_idx in range(len(list_lat)):
str2 = str(gps_week[sav_idx]) + ',' + str(
gps_second[sav_idx]) + ',' + str(
list_lat[sav_idx]) + ',' + str(
list_lon[sav_idx]) + ',' + str(altitude[sav_idx])
print str2
file_object.write(str2)
file_object.write('\n')
print 'llhCpt_', llhCpt_.longitude, type(llhCpt_.longitude)
def callcptBestPos_(self, data): # 1Hz
self.bestPos_ = BESTPOS()
self.bestPos_ = data
self.GrouLat_ = data.latitude
self.GrouLon_ = data.longitude
self.GrouAlt_ = data.altitude
"""
This module publishes the car control message.
"""
import rospy
import threading
import math
from sensor_msgs.msg import Image
from std_msgs.msg import Bool
from car_msgs.msg import ABS, PX2
from novatel_msgs.msg import BESTPOS, INSPVA, Alignment
g_image_msg = Image()
g_can_msg = ABS()
g_rtk_msg = BESTPOS()
g_ins_msg = INSPVA()
g_alig_msg = Alignment()
g_est_msg = PX2()
g_act_msg = PX2()
def est_callback(data):
global g_est_msg
g_est_msg = data
def act_callback(data):
global g_act_msg
g_act_msg = data
def callcptBestPos(self, data):
print 'best pose publish'
self.bestPos_ = BESTPOS()
self.bestPos_ = data
self.Odometry_ = Odometry()
self.Odometry_.header.frame_id = 'map'
self.Odometry_.header.seq = self.bestPos_.header.gps_week_seconds
self.Odometry_.header.stamp = self.Imu_.header.stamp
self.Odometry_.child_frame_id = 'base_link'
self.transENU_ = ()
self.lat_ = self.bestPos_.latitude
self.lon_ = self.bestPos_.longitude
self.alt_ = self.bestPos_.altitude
if (self.oriLat_ == 0):
self.oriLat_ = self.lat_
self.oriLon_ = self.lon_
self.oriAlt_ = self.alt_
self.originLonLatAlt_.append(self.lon_)
self.originLonLatAlt_.append(self.lat_)
self.originLonLatAlt_.append(self.alt_)
self.currentLonLatAlt_.append(self.lon_)
self.currentLonLatAlt_.append(self.lat_)
self.currentLonLatAlt_.append(self.alt_)
# print 'origin ',self.originLonLatAlt_
# print 'current',self.currentLonLatAlt_
self.ENU_ = transformLonLatAltToEnu(tuple(self.originLonLatAlt_),
tuple(self.currentLonLatAlt_))
self.transENU_ = self.transferENU(self.ENU_)
#print 'self.transENU_=',self.transENU_
#print 'ENU_', self.ENU_, math.sqrt(
#self.ENU_[0] * self.ENU_[0] + self.ENU_[1] * self.ENU_[1] + self.ENU_[2] * self.ENU_[2])
self.Odometry_.pose.pose.position.x = self.transENU_[0]
self.Odometry_.pose.pose.position.y = self.transENU_[1]
self.Odometry_.pose.pose.position.z = self.transENU_[2] #self.alt_
Quaternion_ = (self.Imu_.orientation.x, self.Imu_.orientation.y,
self.Imu_.orientation.z, self.Imu_.orientation.w)
euler = tf.transformations.euler_from_quaternion(Quaternion_)
if (self.firsYaw == 0):
self.firsYaw = self.bestPos_.azimuth
#print 'self.firsYaw ',self.firsYaw*180/pi
#print 'euler----',(euler[2]-self.firsYaw)*180/pi
q_orig = quaternion_from_euler(euler[0], euler[1],
(euler[2] - self.firsYaw))
q_rot = quaternion_from_euler(0, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)
eulerNew = tf.transformations.euler_from_quaternion(q_new)
#print 'euler New ----', eulerNew[2]*180/pi
# print type(Quaternion_),Quaternion_,type(q_new),q_new
self.Odometry_.pose.pose.orientation.x = q_new[0]
self.Odometry_.pose.pose.orientation.y = q_new[1]
self.Odometry_.pose.pose.orientation.z = q_new[2]
self.Odometry_.pose.pose.orientation.w = q_new[3]
self.count_ = self.count_ + 1
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.SPHERE
marker.ns = "basic_shapes"
marker.id = self.count_ # id is very important
marker.action = marker.ADD
marker.scale.x = 2.2
marker.scale.y = 2.2
marker.scale.z = 2.2
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = self.transENU_[0] # ground truth
marker.pose.position.y = self.transENU_[1] # ground truth
#if((self.bestPos_.header.gps_week_seconds>176810000) and (self.bestPos_.header.gps_week_seconds<176838000)):
#marker.pose.position.x = self.hdlOdom.pose.pose.position.x + self.CompenOdom.pose.pose.position.x + 0.0001 * (self.bestPos_.header.gps_week_seconds - 176810000)
#marker.pose.position.y = self.hdlOdom.pose.pose.position.y + self.CompenOdom.pose.pose.position.y + 0.0001 * (self.bestPos_.header.gps_week_seconds - 176810000)
marker.pose.position.z = 0
print 'length->', len(self.markerArray.markers)
self.markarraypublisher.publish(self.markerArray)
slamErrorX = marker.pose.position.x - self.ndtPose.pose.position.x
slamErrorY = marker.pose.position.y - self.ndtPose.pose.position.y
vectorLon_ = (math.cos((self.bestPos_.azimuth + 90) * 3.14159 / 180.0),
math.sin((self.bestPos_.azimuth + 90) * 3.14159 / 180.0)
) # heading of vehicle
slamErrorLon = slamErrorX * vectorLon_[0] + slamErrorY * vectorLon_[1]
vectorLat_ = (math.cos(
(self.bestPos_.azimuth + 90 + 90) * 3.14159 / 180.0),
math.sin(
(self.bestPos_.azimuth + 90 + 90) * 3.14159 / 180.0)
) # heading of vehicle
slamErrorLat = slamErrorX * vectorLat_[0] + slamErrorY * vectorLat_[1]
print 'slamErrorLat', slamErrorLat, ' slamErrorLon', slamErrorLon
slamError = math.sqrt(slamErrorX * slamErrorX +
slamErrorY * slamErrorY)
#if((self.bestPos_.header.gps_week_seconds>176810000) and (self.bestPos_.header.gps_week_seconds<176838000)):
#marker.pose.position.x = self.transENU_[0]
#marker.pose.position.y = self.transENU_[1]
self.markerArray.markers.append(marker)
self.Num.append(self.count_)
self.SLAMErrorX.append(math.fabs(slamErrorLat))
self.SLAMErrorY.append(math.fabs(slamErrorLon))
self.SLAMError.append(slamError)
self.ndtReal_.append(self.ndtRealibility)
rows = zip(self.Num, self.SLAMErrorX, self.SLAMErrorY, self.ndtReal_,
self.SLAMError)
with open('SLAMError.csv',
"w") as f: # output the integration positioning error
writer = csv.writer(f)
for row in rows:
writer.writerow(row)
std_ = np.std(self.SLAMError) # stdandard deviation
print 'meanEr_', sum(self.SLAMError) / (len(
self.SLAMError)), 'std_----', std_
self.OdometryPub_.publish(self.Odometry_)
self.calRelativePosition()
print '--------------------------------'
self.currentLonLatAlt_[:] = []