+ 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;
+ }
+ }
+ }
+