Detect type junction

detect_type_junction.cpp

#include "config.h"
#include <iostream>
#include "classes.h"
using namespace std;

//------------------
//Type junction:

//0 Type undetected
//1 Corridor with a sideroad to the right
//2 Corridor with a siderode to the left
//3 T junction
//4 Full junction
//5 Dead end
//6 Turn right
//7 Turn left
//---------------------------
//Initializing values
float dist_wall =1.3;                                                           //Distance to the wall                                                       //Minimum difference between beam lengths
float view_dist2 = VIEW_DIST;                                                   //Maximum range of the Laser Range Finder


int detect_type_junction(emc::LaserData laser) {
    float diffmin = 0.3; //[m]
    int right =0;                                                               //Initial value of right detection
    int left = 0;                                                               //Initial value of left detection
    int straight = 1;                                                           //Initial value of the straight detection

    for(int i=RIGHT_START; i<RIGHT_END; i=i+1){                                               //For laser points right
        if (laser.ranges[i]<VIEW_DIST){                                         //If a point if withing the range
            if (abs(laser.ranges[i]-laser.ranges[i+1])>diffmin && abs(laser.ranges[i]-laser.ranges[i+2])>diffmin && abs(laser.ranges[i]-laser.ranges[i+3])>diffmin){
                right = 1;                                                      //Right value becomes one for given constraint above
                float dist = laser.ranges[i];                                   //The distance of the beams on the right side
                float theta = (laser.angle_min + laser.angle_increment * i) * 67.5 * M_PI / 180;
                view_dist2 = dist*cos(theta) + dist_wall;                       //New maximum distance of the Laser Range Finder to compensate for the straight detection
            }
        }
    }

    for(int i=LEFT_START; i<LEFT_END; i=i+1){                                               //For laser points left
        if (laser.ranges[i]<VIEW_DIST){                                         //If a point if withing the range
            if (abs(laser.ranges[i]-laser.ranges[i-1])>diffmin && abs(laser.ranges[i]-laser.ranges[i-2])>diffmin && abs(laser.ranges[i]-laser.ranges[i-3])>diffmin){
                left = 1;                                                       //Left value becomes one for given constraint above
                float dist = laser.ranges[i];                                   //The distance of the beams on the left side
                float theta = (laser.angle_min + laser.angle_increment * i) * 67.5 * M_PI / 180;
                view_dist2 = dist*cos(theta) + dist_wall;                       //New maximum distance of the Laser Range Finder to compensate for the straight detection
            }
        }
    }

    for(int i=FRONT_START; i<FRONT_END; i=i+1){                                               //For laser points in front
        if (laser.ranges[i]<view_dist2 && laser.ranges[i]>MIN_DIST && laser.ranges[i+1]<view_dist2 && laser.ranges[i+1]>MIN_DIST &&laser.ranges[i+2]<view_dist2 && laser.ranges[i+2]>MIN_DIST){
            straight = 0;                                                       //Straight value is set to zero
        }
    }

    //translate perceptions to junction types
    if(right == 1 &&  left == 0 && straight == 1){
        typearray[iteration] = 1;
    }else if(right == 0 &&  left == 1 && straight == 1){
        typearray[iteration] = 2;
    }else if(right == 1 &&  left == 1 && straight == 0){
        typearray[iteration] = 3;
    }else if(right == 1 &&  left == 1 && straight == 1){
        typearray[iteration] = 4;
    }else if(right == 0 &&  left == 0 && straight == 0){
        typearray[iteration] = 5;
    }else if(right == 1 &&  left == 0 && straight == 0){
        typearray[iteration] = 6;
    }else if(right == 0 &&  left == 1 && straight == 0){
        typearray[iteration] = 7;
    }
    if(typearray[iteration] == 5){                                              //Failsafe for when PICO wrongly detects a dead end because he does not see vertices (i.e. in open space)
        for(int i = RIGHT_START; i<RIGHT_END; i = i+1){                                       //For the right side
            if((laser.ranges[i] > dist_wall || laser.ranges[i]<0.01)&&(laser.ranges[i+1] > dist_wall || laser.ranges[i+1]<0.01)&&(laser.ranges[i+2] > dist_wall || laser.ranges[i+2]<0.01)){
                right = 1;                                                      //Right value becomes one
                typearray[iteration] = 6;                                       //type_junction becomes a turn to the right
            }
        }
        for(int i = LEFT_START; i<LEFT_END; i = i+1){                                       //For the left side
            if((laser.ranges[i] > dist_wall || laser.ranges[i]<0.01)&&(laser.ranges[i+1] > dist_wall || laser.ranges[i+1]<0.01)&&(laser.ranges[i+2] > dist_wall || laser.ranges[i+2]<0.01)){
                left = 1;                                                       //Left value becomes one
                typearray[iteration] = 7;                                       //type_junction becomes a turn to the left

            }
        }
        if (left == 1 && right == 1){                                           //If the left side and right are both equal to one
            typearray[iteration] = 3;                                           //The type_junction becomes a T-junction
        }
    }


    if(iteration > 6){                                                          //With more than six iterations
                                                                                //If the array type gives multiple times the same output
        if((typearray[iteration-6] == typearray[iteration-5]) &&(typearray[iteration-5] == typearray[iteration-4]) &&(typearray[iteration-4] == typearray[iteration-3]) &&(typearray[iteration-3] == typearray[iteration-2]) &&(typearray[iteration-2] == typearray[iteration-1]) && (typearray[iteration-1] == typearray[iteration]) )
        {
            type_junction = typearray[iteration];                               //Assign type_junction to this output
			
            cout << "Junction type detected:"<<typearray[iteration] << endl;
        }
    }
}