EMC snippets

pledge.cpp

#include "pledge.h"
#include "config.h"
#include "math.h"
#include "taskmanager.h"

Pledge checkPledge(int direction, bool space_left, bool space_top, bool space_right)
{
 state_t state;
    // Preferred direction
    if (direction == 0) {
        if (space_top) {
            state = straight_pref;  // Keep going forward
            std::cout << "Dir: " << direction << ", Go forward in preferred direction" << std::endl;
        }
        else if (!space_top) {
            state = turn_left;      // Turn left when obstacle in front
            direction--;
            std::cout << "Dir: " << direction << ", Obstacle ahead, turn left" << std::endl;
        }
        else if (!space_left && !space_top && !space_right) {
            state = turn_180;       // Dead end detected
            direction--;
            direction--;
            std::cout << "Dir: " << direction << ", Dead end detected, rotate 180 deg" << std::endl;
        }
    }
    // Other than preferred direction
    else {
        if (space_right) {
            state = turn_right;     // Always turn right when there is space
            direction++;
            std::cout << "Dir: " << direction << ", Turn right, unpreferred direction" << std::endl;
        }
        else if (!space_right && space_top) {
            state = straight;
            std::cout << "Dir: " << direction << ", Go straight, unpreferred direction" << std::endl;
        }
        else if (!space_right && !space_top && space_left) {
            state = turn_left;
            direction--;
            std::cout << "Dir: " << direction << ", Go left, unpreferred direction" << std::endl;
        }
        else if (!space_right && !space_top && !space_left) {
            state = turn_180;
            direction--;
            direction--;
            std::cout << "Dir: " << direction << ", Dead end detected, rotate 180 deg, unpreferred direction" << std::endl;
        }
    }

    return Pledge(direction, state);
}

spacerecognition.cpp

#include "spacerecognition.h"
#include "config.h"
#include "math.h"

Spaces getSpace(emc::LaserData scan)
{
    bool space_right = true, space_top = true, space_left = true;
    double beam_width = M_PI*BEAM_WIDTH_DEG/180;
    double a;

    for (unsigned int i = 0; i < scan.ranges.size(); i++){
        a = scan.angle_min + i * scan.angle_increment;      // Current angle
        if ((a < -M_PI/2 + beam_width/2) && (a > -M_PI/2 - beam_width/2)){
            // Right of robot, check for obstacles nearby
            if ((scan.ranges[i] < DIST_TRES_MAX) && (scan.ranges[i] > DIST_TRES_MIN)) {
                space_right = false;
            }
        } else if ((a < beam_width/2) && (a > -beam_width/2)) {
            // Top of robot, check for obstacles nearby
            if ((scan.ranges[i] < DIST_FORWARD_TRES_MAX) && (scan.ranges[i] > DIST_TRES_MIN)) {
                space_top = false;
            }
        } else if ((a < M_PI/2 + beam_width/2) && (a > M_PI/2 - beam_width/2)) {
            // Left of robot, check for obstacles nearby
            if ((scan.ranges[i] < DIST_TRES_MAX) && (scan.ranges[i] > DIST_TRES_MIN)) {
                space_left = false;
            }
        }
    }

    return Spaces(space_left, space_top, space_right);
}

bool getRightSpace(emc::LaserData scan, double dist_max)
{
    bool space_right = true;
    double beam_width = M_PI*BEAM_WIDTH_DEG/180;
    double a;

    for (unsigned int i = 0; i < scan.ranges.size(); i++){
        a = scan.angle_min + i * scan.angle_increment;      // Current angle
        if ((a < -M_PI/2 + beam_width/2) && (a > -M_PI/2 - beam_width/2)){
            // Right of robot, check for obstacles nearby
            if ((scan.ranges[i] < dist_max) && (scan.ranges[i] > DIST_TRES_MIN)) {
                space_right = false;
            }
        }
    }

    return space_right;
}

worldmodel.cpp

#include "worldmodel.h"

Location Node::getLocation(){
    return location_;
}

int Node::getType(){
    return type_;
}

int Node::getID(){
    return ID_;
}

int Node::getLRFindex(){
    return LRFindex_;
}

void Node::setID(int node_ID){
    ID_ = node_ID;
}

void Node::setType(int node_type){
    type_ = node_type;
}

void Node::setLocation(Location node_location){
    location_ = node_location;
}

void Node::setLRFindex(int index){
    LRFindex_ = index;
}

void Node::addConnections(int new_ID){
    connections_.push_back(new_ID);
}

void Node::printInfo(){
    std::cout << "Node Info: " << std::endl;
    std::cout << "\tID: " << ID_ << "\n\tType: " << type_ << "\n\tLocation(angle, distance): (" << location_.angle;
    std::cout << " ," << location_.dist << ")" << std::endl;

    std::cout << "\tConnection to other nodes (ID):" << connections_.size() << std::endl;
    for(int connum = 0; connum < connections_.size(); connum++){
        std::cout << "\t" << connections_[connum] << std::endl;
    }
}

// Evaluate each datapoint in the LRF data to indentify nodes
std::vector<Node>  ObjectRecognition::scanNodes(emc::LaserData scan){

    int new_node_ID = 0;
    int nodeID = 1;
    int new_node_type = ENDNODE;
    int scan_range_length = scan.ranges.size();
    bool node_recognized = false;

    Location new_node_loc(0.0, 0.0);
    Node new_node(new_node_ID, new_node_type, new_node_loc);    // Current node considered
    Node prev_node(new_node_ID, new_node_type, new_node_loc);   // Previous node entered into the new_node_vector
    std::vector<Node> new_node_vector;

    // First and last node are endpoint per definition
    new_node_type = ENDNODE;

    // First data point
    new_node_loc.setDist(scan.ranges[0]);
    new_node_loc.setAngle(scan.angle_min);
    new_node.setLocation(new_node_loc);
    new_node.setID(0);
    new_node.setLRFindex(0);
    new_node.setType(new_node_type);
    new_node_vector.push_back(new_node);

    // The other points need to be checked
    for (int i = 1; i < scan_range_length - 1; i++){

        node_recognized = false;
        // Test if datapoint is end node or corner node and add to node list
        if (endNodeTest(scan, i)){
            new_node_type = ENDNODE;;
            node_recognized = true;
        }
        else if (cornerNodeTest(scan, i, CORNERNODELOOP) || cornerEndNodeTest(scan, i)){
            new_node_type = CORNERNODE;
            node_recognized = true;
        }

        if (node_recognized){
            // Set node location
            new_node_loc.setAngle(scan.angle_min + scan.angle_increment*i);
            new_node_loc.setDist(scan.ranges[i]);
            new_node_ID = nodeID;

            // Set new node and add it to the new node vector
            new_node.setID(new_node_ID);
            new_node.setLRFindex(i);
            new_node.setType(new_node_type);
            new_node.setLocation(new_node_loc);

            // Check if nodes are to close.
            // If true do not set new node.
            if (proximityTest(new_node, prev_node) == false){
                new_node_vector.push_back(new_node);
                // Now check if connections need to be added to nodes
                // std::cout << "Node ID: " << new_node.getID() << std::endl;
                if (checkPrevNodeCon(new_node_vector)){
                    addPrevNodeCon(new_node_vector);
                }

                if (new_node.getType() == ENDNODE){
                    nodeID++;
                }
                else if (new_node.getType() == CORNERNODE){
                    nodeID++;
                }
                prev_node = new_node;
            }
        }

    }

    // Last node at last data point
    new_node_ID         = nodeID;
    new_node_type       = ENDNODE;

    new_node_loc.setAngle(scan.angle_min + scan.angle_increment*(scan.ranges.size()-1));
    new_node_loc.setDist(scan.ranges[scan.ranges.size()-1]);
    new_node.setLocation(new_node_loc);
    new_node.setID(nodeID);
    new_node.setLRFindex(scan_range_length - 1);
    new_node.setType(new_node_type);
    new_node_vector.push_back(new_node);
    nodeID++;

    // Now check if connections need to be set
    if(checkPrevNodeCon(new_node_vector)){
        addPrevNodeCon(new_node_vector);
    }
    return new_node_vector;
}

bool ObjectRecognition::endNodeTest(emc::LaserData scan, int index){
    double prev_dist = scan.ranges[index-1];         // Distance of previous datapoint
    double cur_dist  = scan.ranges[index];          // Distance of current datapoint
    double next_dist = scan.ranges[index+1];        // Distance of next datapoint

    if ((cur_dist-next_dist) > ENDNODENORM || cur_dist-prev_dist > ENDNODENORM || (cur_dist-scan.range_max<MAXRANGENORM && cur_dist-scan.range_max>-MAXRANGENORM)){
        return true;
    }
    else{
        return false;
    }

    return true;
}

bool ObjectRecognition::cornerNodeTest(emc::LaserData scan, int index, int nloop){
    int index_count = MINCORNERNODEINDEX;  // Counter for number of indexes offset of data point checked
    int scan_length = scan.ranges.size();   // Number of data samples of LRF
    int max_index = 0;
    int min_index = 0;

    bool max_index_checked = false;
    bool min_index_checked = false;

    // Location of datapoints
    double prev_dist, cur_dist, next_dist, prev_angle, cur_angle, next_angle, xpos_cur, ypos_cur, xpos_next, ypos_next, xpos_prev, ypos_prev, delta_r;
    double a, b, c;
    bool testres = false;

    // Determine maximum index for the corner node check
    // Checks radius and stops looping if no more data is available
    max_index = index_count;
    while((delta_r < MAXCONRNERRADIUS) && index+index_count < scan_length){
        cur_dist = scan.ranges[index];
        cur_angle = scan.angle_min+scan.angle_increment*(index);
        next_dist = scan.ranges[index+index_count];
        next_angle = scan.angle_min+scan.angle_increment*(index+index_count);

        // Position of current data point
        xpos_cur = cos(cur_angle)*cur_dist;
        ypos_cur = sin(cur_angle)*cur_dist;

        // Position of next data point
        xpos_next = cos(next_angle)*next_dist;
        ypos_next = sin(next_angle)*next_dist;

        delta_r = (pow(xpos_cur - xpos_next,2)+pow(ypos_cur - ypos_next,2));
        max_index = index_count;
        max_index_checked = true;
        index_count++;
    }

    // And the minimum index
    index_count = MINCORNERNODEINDEX;
    min_index = index_count;
    delta_r     = 0;
    
    while (delta_r < MAXCONRNERRADIUS && index-index_count > 0){
        cur_dist = scan.ranges[index];
        cur_angle = scan.angle_min+scan.angle_increment*(index);
        prev_dist = scan.ranges[index-index_count];
        prev_angle = scan.angle_min+scan.angle_increment*(index-index_count);

        // Position of current data point
        xpos_cur = cos(cur_angle)*cur_dist;
        ypos_cur = sin(cur_angle)*cur_dist;

        // Position of next data point
        xpos_prev = cos(prev_angle)*prev_dist;
        ypos_prev = sin(prev_angle)*prev_dist;

        delta_r = (pow(xpos_cur - xpos_prev,2)+pow(ypos_cur - ypos_prev,2));
        min_index = index_count;
        min_index_checked = true;
        index_count++;
    }

    // Check if both maximum and minimum index are set correctly, else skip setting the node
    if (max_index_checked && min_index_checked){
        for(int i = 1; i <= nloop; i++){
            prev_dist    = scan.ranges[index-min_index/i];          // Distance of previous datapoint
            cur_dist     = scan.ranges[index];                      // Distance of current datapoint
            next_dist    = scan.ranges[index+max_index/i];          // Distance of next datapoint
            prev_angle   = scan.angle_min+scan.angle_increment*(index-min_index/i);
            cur_angle    = scan.angle_min+scan.angle_increment*(index);
            next_angle   = scan.angle_min+scan.angle_increment*(index+max_index/i);

            a = sqrt(pow(prev_dist,2)+pow(cur_dist,2)-2*prev_dist*cur_dist*cos(cur_angle-prev_angle));
            b = sqrt(pow(next_dist,2)+pow(cur_dist,2)-2*next_dist*cur_dist*cos(next_angle-cur_angle));
            c = sqrt(pow(next_dist,2)+pow(prev_dist,2)-2*next_dist*prev_dist*cos(next_angle-prev_angle));

            if ((pow(a,2)+pow(b,2)-pow(c,2)) <= CORNERNODENORM && (pow(a,2)+pow(b,2)-pow(c,2)) >= -CORNERNODENORM){
                testres = true;
            }
            else{
                testres = false;
                break;
            }
        }
    }
    return testres;
}

bool ObjectRecognition::cornerEndNodeTest(emc::LaserData scan, int index){
    bool testres = false;
    double prev_dist;     // Distance of previous datapoint
    double cur_dist;      // Distance of current datapoint
    double next_dist;     // Distance of next datapoint

    prev_dist    = scan.ranges[index-1];          // Distance of previous datapoint
    cur_dist     = scan.ranges[index];            // Distance of current datapoint
    next_dist    = scan.ranges[index+1];          // Distance of next datapoint

    if ((cur_dist-next_dist) < -ENDNODENORM || cur_dist-prev_dist < -ENDNODENORM){
        testres = true;
    }
    return testres;
}

bool ObjectRecognition::proximityTest(Node new_node, Node prev_node){
    if ((pow(new_node.getLocation().xpos - prev_node.getLocation().xpos,2)+pow(new_node.getLocation().ypos - prev_node.getLocation().ypos,2)) <= pow(FILTERSIZE,2)){
        return true;
    }
    return false;
}

bool ObjectRecognition::doorCheck(std::vector<Node> node_vector, double coordx[], double coordy[])
{
    int NNodes = node_vector.size();      //Number of nodes
    int node1type, node2type, node3type, node4type;
    bool res = false;
    double node1x, node1y, node1r, node1a, node2x, node2y, node2r, node2a;
    double node3x, node3y, node3r, node3a, node4x, node4y, node4r, node4a;
    double dist12, dist23, dist34;
    double a1, b1, c1, a2, b2, c2, a11, a22;
    double L11, L12, L13, L14, L15, L16;
    double L21, L22, L23, L24, L25, L26;
    double point1side, point4side;

    for(int i = 0; i < NNodes-3 ; i++){
        Node node1 = node_vector[i];
        node1x = node1.getLocation().xpos;
        node1y = node1.getLocation().ypos;
        Node node2 = node_vector[i+1];
        node2x = node2.getLocation().xpos;
        node2y = node2.getLocation().ypos;
        Node node3 = node_vector[i+2];
        node3x = node3.getLocation().xpos;
        node3y = node3.getLocation().ypos;
        Node node4 = node_vector[i+3];
        node4x = node4.getLocation().xpos;
        node4y = node4.getLocation().ypos;

        // Check if values are correct
        dist12= sqrt(pow(node1x-node2x,2)+pow(node1y-node2y,2));
        dist23= sqrt(pow(node2x-node3x,2)+pow(node2y-node3y,2));
        dist34= sqrt(pow(node3x-node4x,2)+pow(node3y-node4y,2));

        if (dist12 <= DOOR_DIST+SIDEDOORCHECKMARGINSIDE && dist12 >= 0.3-SIDEDOORCHECKMARGINSIDE && dist34 <= DOOR_DIST+SIDEDOORCHECKMARGINSIDE && dist34 >= 0.3-SIDEDOORCHECKMARGINSIDE && dist23 <= 1.5+DOORCHECKMARGINBACK && dist23 >= 0.5-DOORCHECKMARGINBACK){

            // Check if both not-door-nodes are on the same side of the door
            point1side = (node1x-node2x)*(node3y-node2y)-(node1y-node2y)*(node3x-node2x);
            point4side = (node4x-node2x)*(node3y-node2y)-(node4y-node2y)*(node3x-node2x);

            if ((point1side <= 0 && point4side <=0) || (point1side >= 0 && point4side >=0)){

                // Check if all nodes are the correct type
                node1type = node1.getType();
                node2type = node2.getType();
                node3type = node3.getType();
                node4type = node4.getType();

                if((node1type == CORNERNODE || node1type == ENDNODE) && node2type == CORNERNODE && node3type == CORNERNODE && (node4type == CORNERNODE || node4type == ENDNODE)){

                    // Check if the two middle nodes are around 90 degrees
                    node1r = node1.getLocation().dist;
                    node1a = node1.getLocation().angle;
                    node2r = node2.getLocation().dist;
                    node2a = node2.getLocation().angle;
                    node3r = node3.getLocation().dist;
                    node3a = node3.getLocation().angle;
                    node4r = node4.getLocation().dist;
                    node4a = node4.getLocation().angle;

                    // Values important to check if node3 is 90 degrees
                    a1 = sqrt(pow(node4r,2)+pow(node3r,2)-2*node4r*node3r*cos(node3a-node4a));
                    b1 = sqrt(pow(node2r,2)+pow(node3r,2)-2*node2r*node3r*cos(node2a-node3a));
                    c1 = sqrt(pow(node2r,2)+pow(node4r,2)-2*node2r*node4r*cos(node2a-node4a));

                    // Values important to check if node2 is 90 degrees
                    a2 = sqrt(pow(node3r,2)+pow(node2r,2)-2*node3r*node2r*cos(node2a-node3a));
                    b2 = sqrt(pow(node1r,2)+pow(node2r,2)-2*node1r*node2r*cos(node1a-node2a));
                    c2 = sqrt(pow(node1r,2)+pow(node3r,2)-2*node1r*node3r*cos(node1a-node3a));

                    if(((pow(a1,2)+pow(b1,2)-pow(c1,2)) <= SIDEDOORCORNERNODENORM && (pow(a1,2)+pow(b1,2)-pow(c1,2)) >= -SIDEDOORCORNERNODENORM) && ((pow(a2,2)+pow(b2,2)-pow(c2,2)) <= SIDEDOORCORNERNODENORM && (pow(a2,2)+pow(b2,2)-pow(c2,2)) >= -SIDEDOORCORNERNODENORM)){

                        // Check if in area. These are four different checks. They state that you are always having one node A in front of you. If you are in the area, Node A+1 should lie on the left and A-1 should lie on the right. A+2 should lie behind you.
                        // Calculate the point "3" of the square
                        L11 = node3x-node4x;
                        L12 = node3y-node4y;
                        L13 = sqrt(pow(L11,2)+pow(L12,2));
                        a11 = atan(L12/L11);
                        L14 = DOOR_DIST;
                        L15 = cos(a11)*L14;
                        L16 = sin(a11)*L14;

                        // Calculate the point "4" of the square
                        L21 = node2x-node1x;
                        L22 = node2y-node1y;
                        L23 = sqrt(pow(L21,2)+pow(L22,2));
                        a22 = atan(L22/L21);
                        L24 = DOOR_DIST;
                        L25 = cos(a22)*L24;
                        L26 = sin(a22)*L24;

                        // Define the coordinates of the door area
                        coordx[0] = node2x;
                        coordy[0] = node2y;

                        coordx[1] = node3x;
                        coordy[1] = node3y;

                        coordx[2] = node3x-L15;
                        coordy[2] = node3y-L16;

                        coordx[3] = node2x-L25;
                        coordy[3] = node2y-L26;

                        if (coordx[0] > 0 && coordy[3] < 0 && coordx[2] < 0 && coordy[1] > 0){
                            res = true;
                        }
                        else if (coordx[3] > 0 && coordy[2] < 0 && coordx[1] < 0 && coordy[0] > 0){
                            res = true;
                        }
                        else if (coordx[2] > 0 && coordy[1] < 0 && coordx[0] < 0 && coordy[3] > 0){
                            res = true;
                        }
                        else if (coordx[1] > 0 && coordy[0] < 0 && coordx[3] < 0 && coordy[2] > 0){
                            res = true;
                        }
                    }
                }
            }
        }
    }
    return res;
}

bool ObjectRecognition::checkPrevNodeCon(std::vector<Node> node_vector){
    // Check if node vector is large enough to check
    if (node_vector.size() > 1){
        // Define index of previous and next node and check if there are datapoints inbetween
        int cur_index   = node_vector.size() - 1;
        int prev_index  = cur_index - 1;
        int cur_LRF_index   = node_vector[cur_index].getLRFindex();
        int prev_LRF_index  = node_vector[prev_index].getLRFindex();
        int LRF_index_diff  = cur_LRF_index - prev_LRF_index;

        // If the LRF index difference is smaller than NODECONINDEXDIFF than the nodes are NOT connected.
        if (LRF_index_diff > NODECONINDEXDIFF){
            return true;
        }else{
            return false;
        }
    }else{
        return false;
    }
}

void ObjectRecognition::addPrevNodeCon(std::vector<Node> &node_vector){
    // Check if node vector is large enough to check
    if (node_vector.size() > 1){
        // Get pointer to nodes in vector
        int cur_index   = node_vector.size() - 1;
        int prev_index  = cur_index - 1;

        int cur_id = node_vector[cur_index].getID();
        int prev_id = node_vector[prev_index].getID();

        // Set connection vector of previous and current node
        node_vector[cur_index].addConnections(prev_id);
        node_vector[prev_index].addConnections(cur_id);
    }
}