4.1.1: Update python Global Localization samples

stereolabs · May 7, 2024 · c7b755b · c7b755b
1 parent 301ac04
commit c7b755b
Show file tree

Hide file tree

Showing 4 changed files with 82 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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: