Skip to content

Commit

Permalink
Fixed python global localization samples
Browse files Browse the repository at this point in the history
  • Loading branch information
@alassagne-sl
alassagne-sl committed May 10, 2024
1 parent c7b755b commit 92210d0
Show file tree
Hide file tree
Showing 4 changed files with 94 additions and 22 deletions.
16 changes: 10 additions & 6 deletions global localization/live/python/gnss_reader/gpsd_reader.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,12 +106,16 @@ def getNextGNSSValue(self):
return current_gnss_data
elif "class" in gpsd_data and gpsd_data["class"] == "SKY":
nb_low_snr = 0
for satellite in gpsd_data['satellites']:
if satellite['used'] and satellite['ss'] < 16:
nb_low_snr += 1
if nb_low_snr > 0:
print("[Warning] Low SNR (<16) on {} satellite(s) (using {} out of {} visible)".format(nb_low_snr, gpsd_data['uSat'], gpsd_data['nSat']))
return self.getNextGNSSValue()
if 'satellites' in gpsd_data:
for satellite in gpsd_data['satellites']:
if satellite['used'] and satellite['ss'] < 16:
nb_low_snr += 1
if nb_low_snr > 0:
if 'uSat' in gpsd_data and 'nSat' in gpsd_data:
print("[Warning] Low SNR (<16) on {} satellite(s) (using {} out of {} visible)".format(nb_low_snr, gpsd_data['uSat'], gpsd_data['nSat']))
else:
print("[Warning] Low SNR (", nb_low_snr, "< 16 )")
return self.getNextGNSSValue()
else:
print("Fix lost: GNSS reinitialization")
self.initialize()
Expand Down
89 changes: 78 additions & 11 deletions global localization/playback/python/gnss_replay.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,31 +37,98 @@ def initialize(self):
gnss_data_point_json = json.loads(d.get_content_as_string())
gnss_data_point_formatted = {}

latitude = gnss_data_point_json["Geopoint"]["Latitude"]
longitude = gnss_data_point_json["Geopoint"]["Longitude"]
altitude = gnss_data_point_json["Geopoint"]["Altitude"]
# We handle 2 formats:
# *
# * {
# "coordinates": {
# "latitude": XXX,
# "longitude": XXX,
# "altitude": XXX
# },
# "ts": 1694263390000000,
# "latitude_std": 0.51,
# "longitude_std": 0.51,
# "altitude_std": 0.73,
# "position_covariance": [
# 0.2601,
# 0,
# 0,
# 0,
# 0.2601,
# 0,
# 0,
# 0,
# 0.5328999999999999
# ]
# },
# *********
# * Or
# * this one will be converted to the format above
# {
# "Eph": 0.467,
# "EpochTimeStamp": 1694266998000000,
# "Epv": 0.776,
# "Geopoint": {
# "Altitude": XXX,
# "Latitude": XXX,
# "Longitude": XXX
# },
# "Position": [
# [
# XXX,
# XXX,
# XXX
# ]
# ],
# "Velocity": [
# [
# -0.63,
# 0.25,
# 0.53
# ]
# ]
# }


if 'Geopoint' in gnss_data_point_json:
latitude = gnss_data_point_json["Geopoint"]["Latitude"]
longitude = gnss_data_point_json["Geopoint"]["Longitude"]
altitude = gnss_data_point_json["Geopoint"]["Altitude"]

gnss_data_point_formatted["ts"] = gnss_data_point_json["EpochTimeStamp"]

latitude_std = gnss_data_point_json["Eph"]
longitude_std = gnss_data_point_json["Eph"]
altitude_std = gnss_data_point_json["Epv"]

else:
latitude = gnss_data_point_json["coordinates"]["latitude"]
longitude = gnss_data_point_json["coordinates"]["longitude"]
altitude = gnss_data_point_json["coordinates"]["altitude"]


gnss_data_point_formatted["ts"] = gnss_data_point_json["ts"]

latitude_std = gnss_data_point_json["latitude_std"]
longitude_std = gnss_data_point_json["longitude_std"]
altitude_std = gnss_data_point_json["altitude_std"]


gnss_data_point_formatted["coordinates"] = {
"latitude": latitude,
"longitude": longitude,
"altitude": altitude
}

gnss_data_point_formatted["ts"] = gnss_data_point_json["EpochTimeStamp"]

latitude_std = gnss_data_point_json["Eph"]
longitude_std = gnss_data_point_json["Eph"]
altitude_std = gnss_data_point_json["Epv"]

gnss_data_point_formatted["latitude_std"] = latitude_std
gnss_data_point_formatted["longitude_std"] = longitude_std
gnss_data_point_formatted["altitude_std"] = altitude_std

gnss_data_point_formatted["position_covariance"] = {
longitude_std + longitude_std, 0, 0, 0, latitude_std + latitude_std, 0, 0, 0,
altitude_std + altitude_std
}

}
if "mode" in gnss_data_point_json:
gnss_data_point_formatted["mode"] = gnss_data_point_json["mode"]
if "status" in gnss_data_point_json:
Expand Down
9 changes: 5 additions & 4 deletions global localization/recording/python/exporter/gnss_saver.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@ def convert_gnss_data_2_json(gnss_data: sl.GNSSData) -> json:
gnss_measure["latitude_std"] = np.sqrt(position_covariance[1 * 3 + 1])
gnss_measure["altitude_std"] = np.sqrt(position_covariance[2 * 3 + 2])

gnss_measure["mode"] = gnss_data.gnss_mode
gnss_measure["status"] = gnss_data.gnss_status
gnss_measure["mode"] = gnss_data.gnss_mode.value
gnss_measure["status"] = gnss_data.gnss_status.value

return gnss_measure

Expand All @@ -40,9 +40,10 @@ def addGNSSData(self, gnss_data):
if self._zed is not None:
data = sl.SVOData()
data.key = "GNSS_json"
data.set_content(convert_gnss_data_2_json(gnss_data))
data.set_string_content(json.dumps(convert_gnss_data_2_json(gnss_data)))
data.timestamp_ns = gnss_data.ts

self._zed.ingest_data_in_svo(data)
self._zed.ingest_data_into_svo(data)

else:
self.all_gnss_data.append(gnss_data)
Expand Down
2 changes: 1 addition & 1 deletion global localization/recording/python/gnss_reader/gpsd_reader.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ def getNextGNSSValue(self):

sl_status = sl.GNSS_STATUS.UNKNOWN
if 'status' in gpsd_data:
gpsd_status = cgpsd_data["status"]
gpsd_status = gpsd_data["status"]
if gpsd_status == 0: # STATUS_UNK
sl_status = sl.GNSS_STATUS.UNKNOWN
elif gpsd_status == 1: # STATUS_GPS
Expand Down

0 comments on commit 92210d0

Please sign in to comment.