Stop robot if point cloud is invalid

hopkira · Nov 20, 2022 · 85b79ed · 85b79ed
1 parent 6223016
commit 85b79ed
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/k9_oak_pipe.py b/k9_oak_pipe.py
@@ -125,9 +125,11 @@ def record_point_cloud(self,depth_image):
         totals = self.bpc.populate_bins(depth_image)
         focus = totals[0:7,10:15]
         min_dist = float(np.nanmin(focus))
-        if min_dist:
+        if np.isnan(min_dist):
+            min_dist = 0.0
+        else:
             min_dist = float(min_dist/1000.0)
-            mem.storeState("forward", min_dist)
+        mem.storeState("forward", min_dist)
         # for each column in the array, find out the closest
         # bin; as the robot cannot duck or jump, the
         # y values are irrelevant