fix lidar JSON encoding

Author: DaAwesomeP

arenarobot/service/sensor/vl53l5cx.py

@@ -11,13 +11,15 @@
 """
 
 import time
+from json import dumps, loads
 from typing import Sequence
 
 import numpy as np
 from periphery import GPIO
 
 from arenarobot.service.sensor import ArenaRobotServiceSensor
 from vl53l5cx_py.driver import VL53L5CX
+from vl53l5cx_py.helpers import VL53L5CXJSONEncoder
 
 
 class ArenaRobotServiceSensorVL53L5CX(ArenaRobotServiceSensor):
@@ -81,9 +83,6 @@ def fetch(self):
         data = []
         for sensor in self.sensors:
             ranges = sensor.get_range()
-            # Remove byte-type data (not JSON-serializable)
-            del ranges["nb_target_detected"]
-            del ranges["reflectance"]
-            del ranges["target_status"]
             data.append(ranges)
+        data = loads(dumps(data, cls=VL53L5CXJSONEncoder))
         self.publish({"data": data})

vl53l5cx_py/cython/vl53l5cx_py_wrapper.pyx

@@ -128,7 +128,8 @@ cdef class VL53L5CX:
         cdef int32_t status = vl53l5cx_py_get_range(&self.dev_conf, &results)
         if status != 0:
             raise RuntimeError
+
         return VL53L5CXSensorData(
             distance_mm=results.distance_mm,
-            target_status=results.target_status
+            target_status=np.frombuffer(results.target_status, dtype='S1')
         )

vl53l5cx_py/helpers.py

@@ -11,6 +11,7 @@
 """
 
 from dataclasses import dataclass
+from json import JSONEncoder
 from typing import List
 
 import numpy as np
@@ -41,3 +42,19 @@ class VL53L5CXSensorData():
 # 		203.20,210.96,225.00,251.55,288.45,315.00,329.04,336.80,
 # 		215.40,225.00,239.04,258.69,281.31,300.96,315.00,324.60,
 # 		225.00,234.60,246.80,261.87,278.13,293.20,305.40,315.00]
+
+class VL53L5CXJSONEncoder(JSONEncoder):
+    """JSON Encoder helper for VL53L5CX packets."""
+
+    def default(self, obj):
+        """JSON Encoder helper function for VL53L5CX packets."""
+        if isinstance(obj, np.ndarray):
+            return obj.tolist()
+
+        elif isinstance(obj, bytes):
+            return int.from_bytes(obj, "little")
+
+        elif isinstance(obj, VL53L5CXSensorData):
+            return vars(obj)
+
+        return JSONEncoder.default(self, obj)