naive obstacle avoidance

diff --git a/cpp/sampleOpenCV/sampleOpenCV.cpp b/cpp/sampleOpenCV/sampleOpenCV.cpp
index 93384ea..8a085fe 100644 (file)
--- a/cpp/sampleOpenCV/sampleOpenCV.cpp
+++ b/cpp/sampleOpenCV/sampleOpenCV.cpp
@@ -50,6 +50,7 @@ class MyListener : public IDepthDataListener
     bool normblurImage = true;
 
     const uint8_t confidence_threshold_ = 0;
+    const double distance_threshold_ = 50;
 
 public :
 
@@ -146,6 +147,9 @@ public :
             std::cout << "col" << col << "   min:" << min << "("<<latest_min_distance_diff_[col]<<")"<< "   max:" << max << std::endl;
         }
 
+        //naive obstacle avoidance demo
+        naiveObstacleAvoidanceDemo();
+
         // scale and display the depth image
         scaledZImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
         resize (zImage8, scaledZImage, scaledZImage.size());
@@ -218,6 +222,41 @@ private:
         return newGrayValue;
     }
 
+    bool naiveIsObstaclePresent(bool free_paths[num_dist_columns_])
+    {
+        bool rv=false;
+        for (uint32_t col=0; col<num_dist_columns_; col++)
+        {
+            free_paths[col] = latest_min_distance_[col] > distance_threshold_;
+            if (!free_paths[col])
+            {
+                rv=true;
+            }
+        }
+        return rv;
+    }
+
+    void naiveObstacleAvoidanceDemo()
+    {
+        bool free_paths[num_dist_columns_];
+        if (naiveIsObstaclePresent(free_paths))
+        {
+            assert(num_dist_columns_ == 4);
+            if (free_paths[1] && free_paths[2])
+            {
+                std::cout << "GOSTRAIGHT" << std::endl;
+            } else if (free_paths[0])
+            {
+                std::cout << "GOLEFT" << std::endl;
+            } else if (free_paths[3])
+            {
+                std::cout << "GORIGHT" << std::endl;
+            } else {
+                std::cout << "STOP" << std::endl;
+            }
+        }
+    }
+
     // define images for depth and gray
     // and for their 8Bit and scaled versions
     Mat zImage, zImage8, scaledZImage;