static bool ObstacleDetection()
{
static int false_count = 0;
static bool prev_detection = false;
//_closest_waypoint = GetClosestWaypoint();
int closest_waypoint = _path_dk.getClosestWaypoint();
std::cout << "closest_waypoint : " << closest_waypoint << std::endl;
DisplayDetectionRange(closest_waypoint + 1);
int vscan_result = vscanDetection(closest_waypoint);
if (prev_detection == false) {
if (vscan_result != -1) {
DisplayObstacleWaypoint(vscan_result);
std::cout << "obstacle waypoint : " << vscan_result << std::endl << std::endl;
prev_detection = true;
_obstacle_waypoint = vscan_result;
SoundPlay();
false_count = 0;
return true;
} else {
prev_detection = false;
return false;
}
} else { //prev_detection = true
if (vscan_result != -1) {
DisplayObstacleWaypoint(vscan_result);
std::cout << "obstacle waypoint : " << vscan_result << std::endl << std::endl;
prev_detection = true;
_obstacle_waypoint = vscan_result;
false_count = 0;
return true;
} else {
false_count++;
std::cout << "false_count : "<<false_count << std::endl;
}
//fail-safe
if (false_count == LOOP_RATE * 2) {
_obstacle_waypoint = -1;
false_count = 0;
prev_detection = false;
return false;
} else {
std::cout << "obstacle waypoint : " << _obstacle_waypoint << std::endl << std::endl;
DisplayObstacleWaypoint(_obstacle_waypoint);
prev_detection = true;
return true;
}
}
}
static EControl ObstacleDetection()
{
static int false_count = 0;
static EControl prev_detection = KEEP;
std::cout << "closest_waypoint : " << _closest_waypoint << std::endl;
std::cout << "current_velocity : " << mps2kmph(_current_vel) << std::endl;
EControl vscan_result = vscanDetection();
DisplayDetectionRange(vmap.getDetectionCrossWalkID(), _closest_waypoint, vscan_result);
if (prev_detection == KEEP) {
if (vscan_result != KEEP) { // found obstacle
DisplayObstacle(vscan_result);
std::cout << "obstacle waypoint : " << _obstacle_waypoint << std::endl << std::endl;
prev_detection = vscan_result;
//SoundPlay();
false_count = 0;
return vscan_result;
} else { // no obstacle
prev_detection = KEEP;
return vscan_result;
}
} else { //prev_detection = STOP or DECELERATE
if (vscan_result != KEEP) { // found obstacle
DisplayObstacle(vscan_result);
std::cout << "obstacle waypoint : " << vscan_result << std::endl << std::endl;
prev_detection = vscan_result;
false_count = 0;
return vscan_result;
} else { // no obstacle
false_count++;
std::cout << "false_count : "<< false_count << std::endl;
//fail-safe
if (false_count >= LOOP_RATE/2) {
_obstacle_waypoint = -1;
false_count = 0;
prev_detection = KEEP;
return vscan_result;
} else {
std::cout << "obstacle waypoint : " << _obstacle_waypoint << std::endl << std::endl;
DisplayObstacle(OTHERS);
return prev_detection;
}
}
}
}