From c7b755b674bbf33a192dd64647c3aed99e44c062 Mon Sep 17 00:00:00 2001 From: antoine Date: Tue, 7 May 2024 09:15:01 +0200 Subject: [PATCH] 4.1.1: Update python Global Localization samples --- .../live/python/gnss_reader/gpsd_reader.py | 40 +++++++++++++++++++ global localization/live/python/live.py | 2 +- .../python/gnss_reader/gpsd_reader.py | 40 +++++++++++++++++++ .../recording/python/recording.py | 2 +- 4 files changed, 82 insertions(+), 2 deletions(-) diff --git a/global localization/live/python/gnss_reader/gpsd_reader.py b/global localization/live/python/gnss_reader/gpsd_reader.py index c0d5a3f4..891d2d41 100644 --- a/global localization/live/python/gnss_reader/gpsd_reader.py +++ b/global localization/live/python/gnss_reader/gpsd_reader.py @@ -46,6 +46,46 @@ def getNextGNSSValue(self): current_gnss_data.longitude_std = 0.001 current_gnss_data.latitude_std = 0.001 current_gnss_data.altitude_std = 1.0 + + gpsd_mode = gpsd_data["mode"] + sl_mode = sl.GNSS_MODE.UNKNOWN + + if gpsd_mode == 0: # MODE_NOT_SEEN + sl_mode = sl.GNSS_MODE.UNKNOWN + elif gpsd_mode == 1: # MODE_NO_FIX + sl_mode = sl.GNSS_MODE.NO_FIX + elif gpsd_mode == 2: # MODE_2D + sl_mode = sl.GNSS_MODE.FIX_2D + elif gpsd_mode == 3: # MODE_3D + sl_mode = sl.GNSS_MODE.FIX_3D + + sl_status = sl.GNSS_STATUS.UNKNOWN + if 'status' in gpsd_data: + gpsd_status = cgpsd_data["status"] + if gpsd_status == 0: # STATUS_UNK + sl_status = sl.GNSS_STATUS.UNKNOWN + elif gpsd_status == 1: # STATUS_GPS + sl_status = sl.GNSS_STATUS.SINGLE + elif gpsd_status == 2: # STATUS_DGPS + sl_status = sl.GNSS_STATUS.DGNSS + elif gpsd_status == 3: # STATUS_RTK_FIX + sl_status = sl.GNSS_STATUS.RTK_FIX + elif gpsd_status == 4: # STATUS_RTK_FLT + sl_status = sl.GNSS_STATUS.RTK_FLOAT + elif gpsd_status == 5: # STATUS_DR + sl_status = sl.GNSS_STATUS.SINGLE + elif gpsd_status == 6: # STATUS_GNSSDR + sl_status = sl.GNSS_STATUS.DGNSS + elif gpsd_status == 7: # STATUS_TIME + sl_status = sl.GNSS_STATUS.UNKNOWN + elif gpsd_status == 8: # STATUS_SIM + sl_status = sl.GNSS_STATUS.UNKNOWN + elif gpsd_status == 9: # STATUS_PPS_FIX + sl_status = sl.GNSS_STATUS.SINGLE + + + current_gnss_data.gnss_mode = sl_mode.value + current_gnss_data.gnss_status = sl_status.value position_covariance = [ gpsd_data["eph"] * gpsd_data["eph"], diff --git a/global localization/live/python/live.py b/global localization/live/python/live.py index 50b5db56..c3b9d3e9 100644 --- a/global localization/live/python/live.py +++ b/global localization/live/python/live.py @@ -123,7 +123,7 @@ def main(): # and GNSS. When the initialization is finish the getGeoPose will return sl.POSITIONAL_TRACKING_STATE.OK current_geopose = sl.GeoPose() current_geopose_satus = fusion.get_geo_pose(current_geopose) - if current_geopose_satus == sl.GNSS_CALIBRATION_STATE.CALIBRATED: + if current_geopose_satus == sl.GNSS_FUSION_STATUS.OK: viewer.updateGeoPoseData(current_geopose, zed.get_timestamp(sl.TIME_REFERENCE.CURRENT)) """ else: diff --git a/global localization/recording/python/gnss_reader/gpsd_reader.py b/global localization/recording/python/gnss_reader/gpsd_reader.py index ffff9356..46592603 100644 --- a/global localization/recording/python/gnss_reader/gpsd_reader.py +++ b/global localization/recording/python/gnss_reader/gpsd_reader.py @@ -52,6 +52,46 @@ def getNextGNSSValue(self): current_gnss_data.latitude_std = 0.001 current_gnss_data.altitude_std = 1.0 + gpsd_mode = gpsd_data["mode"] + sl_mode = sl.GNSS_MODE.UNKNOWN + + if gpsd_mode == 0: # MODE_NOT_SEEN + sl_mode = sl.GNSS_MODE.UNKNOWN + elif gpsd_mode == 1: # MODE_NO_FIX + sl_mode = sl.GNSS_MODE.NO_FIX + elif gpsd_mode == 2: # MODE_2D + sl_mode = sl.GNSS_MODE.FIX_2D + elif gpsd_mode == 3: # MODE_3D + sl_mode = sl.GNSS_MODE.FIX_3D + + sl_status = sl.GNSS_STATUS.UNKNOWN + if 'status' in gpsd_data: + gpsd_status = cgpsd_data["status"] + if gpsd_status == 0: # STATUS_UNK + sl_status = sl.GNSS_STATUS.UNKNOWN + elif gpsd_status == 1: # STATUS_GPS + sl_status = sl.GNSS_STATUS.SINGLE + elif gpsd_status == 2: # STATUS_DGPS + sl_status = sl.GNSS_STATUS.DGNSS + elif gpsd_status == 3: # STATUS_RTK_FIX + sl_status = sl.GNSS_STATUS.RTK_FIX + elif gpsd_status == 4: # STATUS_RTK_FLT + sl_status = sl.GNSS_STATUS.RTK_FLOAT + elif gpsd_status == 5: # STATUS_DR + sl_status = sl.GNSS_STATUS.SINGLE + elif gpsd_status == 6: # STATUS_GNSSDR + sl_status = sl.GNSS_STATUS.DGNSS + elif gpsd_status == 7: # STATUS_TIME + sl_status = sl.GNSS_STATUS.UNKNOWN + elif gpsd_status == 8: # STATUS_SIM + sl_status = sl.GNSS_STATUS.UNKNOWN + elif gpsd_status == 9: # STATUS_PPS_FIX + sl_status = sl.GNSS_STATUS.SINGLE + + + current_gnss_data.gnss_mode = sl_mode.value + current_gnss_data.gnss_status = sl_status.value + position_covariance = [ gpsd_data["eph"] * gpsd_data["eph"], 0.0, diff --git a/global localization/recording/python/recording.py b/global localization/recording/python/recording.py index 3c10cf21..8d23d68d 100644 --- a/global localization/recording/python/recording.py +++ b/global localization/recording/python/recording.py @@ -151,7 +151,7 @@ def main(): # and GNSS. When the initialization is finish the getGeoPose will return sl.POSITIONAL_TRACKING_STATE.OK current_geopose = sl.GeoPose() current_geopose_satus = fusion.get_geo_pose(current_geopose) - if current_geopose_satus == sl.GNSS_CALIBRATION_STATE.OK: + if current_geopose_satus == sl.GNSS_FUSION_STATUS.OK: viewer.updateGeoPoseData(current_geopose, zed.get_timestamp(sl.TIME_REFERENCE.CURRENT)) """ else: