detection.cpp

detection.cpp

#include "detection.h"

// constructors
//===================================
Detection::Detection() {
    // mprocessing
    plot = true;    // Turn on plotting if true;

    // sitrec
    tol_filter = 2;
    filter = 0;

    // slam
}

// getters
//===================================
Point Detection::getCurrentLocation() {
    return curloc;
}


std::vector<Point> Detection::getCorners () {
    return corners;
}


Situation Detection::getSituation() {
    return sit;
}

// setters
//===================================


// monitors
//===================================
bool Detection::monitor() {
    //std::cout << std::sqrt(std::pow(sit.x,2)+std::pow(sit.y,2)) << std::endl;
    // if crossroads and distance tolerance
    if (sit.forward && sit.left && sit.right && std::sqrt(std::pow(sit.x,2)+std::pow(sit.y,2)) < 0.7) {
        return true;
    }
    // if T-junction with forward and left OR right is open
    else if ((sit.forward && (sit.left || sit.right)) && sit.left != sit.right  && std::sqrt(std::pow(sit.x,2)+std::pow(sit.y,2)) < 0.6) {
        return true;
    }
    // if T-junction with left and right open
    else if ( sit.forward == false && (sit.left && sit.right) && std::sqrt(std::pow(sit.x,2)+std::pow(sit.y,2)) < 0.2) {
        return true;
    }
    // if deadend
    else if (sit.left == false && sit.right == false && sit.forward == false && std::sqrt(std::pow(sit.x,2)+std::pow(sit.y,2)) < 0.4) {

    }
    else {
        return false;
    }
}

// configurators
//===================================
void Detection::ResetOdomOrigin(Point current_location){
    origin.x = current_location.x;
    origin.y = current_location.y;
    origin.a = current_location.a;
}

void Detection::ResetOdomOrigin(emc::OdometryData current_location){
    origin.x = current_location.x;
    origin.y = current_location.y;
    origin.a = current_location.a;
}

// coordinator/composer
//===================================
void Detection::detection(emc::IO &io, emc::LaserData scan, emc::OdometryData odom) {

    //COMMUNICATION:
    //read most recent odometry data and laser data:
    io.readOdometryData(odom);
    io.readLaserData(scan);

    //Determine current location by correcting for origin offset:
    curloc.x = odom.x - origin.x;
    curloc.y = odom.y - origin.y;
    curloc.a = odom.a - origin.a;

    // Make sure that the angle of curloc lies between -PI and PI:
    while(curloc.a >= PI || curloc.a < -PI){ //keep looping if curloc is outside range
        if(curloc.a >= 0){  // if curloc is too large, substract 2pi
            curloc.a = curloc.a - 2*PI;
        }
        else {//curloc.a < 0   // if curloc is too small, add 2pi
            curloc.a = curloc.a + 2*PI;
        }
    }

    //COMPUTATIONAL ENTITIES:
    Detection::mprocessing(io, scan, odom);    // trigger computational entity 'mprocessing'
//    Detection::sitrec(io, scan, odom_cor);         // trigger computational entity 'situation recognition'
//    Detection::slam(io, scan, odom_cor);           // trigger computational entity 'slam'
}

// computational entities
//===================================