Supervisor

Supervisor snippet

#include <vector>
#include <assert.h>
#include <tf2/LinearMath/Vector3.h>
#include <emc/io.h>

#include <cstdlib>
#include <cmath>
#include <robo/supervisor.h>
#include <robo/sensing.h>
#include <robo/debugging.h>
#include <robo/settings.h>
#include <robo/node_recognizing.h>
#include <robo/pledge.h>
#include <robo/track_point.h>
#include <robo/doors.h>


#include <geometry_msgs/Twist.h>
#include <ros/node_handle.h>

int rightLaserBeam1 = 107;
int leftLaserBeam1 = 892;
int rightLaserBeam2 = 110;
int leftLaserBeam2 = 895;
float marg;
float marg2;
int numGaps;
// Same values of node_recognizing, perhaps combine them?:
tf2::Vector3 midpointXY;
direction_t pledgeDirection;                // Stored pledge direction
int wait_counter;
state_t predoor_state;

#define RIGHT_LEFT_ANGLE_TOLERANCE 0.0
#define MID_JUNCTION 0.18
#define WAIT_TIME 5 //s
const int wait_iterations = WAIT_TIME * RATE;
bool leaving_door;
bool escape_dead_end;
double angle_pre_uturn_left, angle_pre_uturn_right;

state_t determine_state(emc::LaserData scan, emc::IO &io, state_t state, std::vector<double> rayToAngle, double angleMargin)
{
    // Same values of node_recognizing, perhaps combine them?:

    double rightLaserAngle = -90;
    double leftLaserAngle = 90;
    double midLaserAngle = 0;

    int rightLaserBeam, leftLaserBeam, midLaserBeam;

    for (unsigned int ii = 0; ii < rayToAngle.size(); ++ii)
    {
        if (abs(rayToAngle[ii] - rightLaserAngle) < angleMargin)
        {
            rightLaserBeam = ii;
        }
        else if (abs(rayToAngle[ii] - midLaserAngle) < angleMargin)
        {
            midLaserBeam = ii;
        }
        else if (abs(rayToAngle[ii] - leftLaserAngle) < angleMargin)
        {
            leftLaserBeam = ii;
        }
    }

    double midpointX, midpointY;
    std::vector<bool> isGapRays;
    std::vector<int> discoveredDoorCorners, discoveredNodes;
    junction_t foundNode;
    double angle_rad, doorAngle, angleToMidRay, angle_deg;
    int doorCenter;
    double angle_uturn_left= -360, angle_uturn_right = 360;
    std::vector<int> midRays;
    if (!leaving_door && !escape_dead_end)
    {
        if ((doorExist(scan)==1) &&
                (state!=CHECK_DOOR) &&
                (state!=PREPARE_FOR_DOOR) &&
                (state!=DOOR_FOUND))
        {
            log<LOG_INFO>("Door supervisor: Found door! Moving to DOOR_FOUND state.");
            state=DOOR_FOUND;
        }
    }

    switch (state)
    {
        case DEAD_END_FOUND:
            state = DOOR_FOUND;
            break;
        case LEAVE_DEAD_END:
            foundNode = node_recognizing(scan, rayToAngle, angleMargin);
            if (foundNode == DEAD_END)
            {
                pledgeDirection = pledge(foundNode);

                //TODO: Make more robust
                if (pledgeDirection == LEFT)
                {
                    log<LOG_INFO>("Supervisor: Escaping with U_TURN_LEFT");
                    state = U_TURN_LEFT;
                }
                else
                {
                    log<LOG_INFO>("Supervisor: Escaping with U_TURN_RIGHT");
                    state = U_TURN_RIGHT;
                }
            }
            else
            {
                log<LOG_INFO>("Supervisor: Escaping with LOOKING_FOR_SIDE");
                state = LOOKING_FOR_SIDE;
            }
            break;

        case DOOR_FOUND:
            log<LOG_INFO>("Supervisor: Door found, prepare for door");
            state=PREPARE_FOR_DOOR;
            break;
        case PREPARE_FOR_DOOR:
            wait_counter = 0;
            if (predoor_state != DEAD_END_FOUND)
            {
                doorCenter = findDoorMidRay(scan);
                if (doorCenter > 0)
                {
                    doorAngle = rayToAngle[doorCenter];
                    if (doorAngle > 75 || doorAngle < -75 || abs(doorAngle) > 150)
                    {
                        log<LOG_INFO>("Supervisor: Preparing for door done, angle at %f. Check door") % doorAngle;
                        state=CHECK_DOOR;
                    }
                }
                else
                {
                    log<LOG_INFO>("Supervisor: Preparing for door done, lost door corners");
                    state=CHECK_DOOR;
                }
            }
            else
            {
                log<LOG_INFO>("Supervisor: Dead end door. Preparing for door done.");
                state=CHECK_DOOR;
            }
            break;
        case CHECK_DOOR://ring bell and check if door opens
            if (wait_counter == 0)
                ringBell(io);
            if (wait_counter < wait_iterations)
            {
                if (wait_counter % RATE == 0 || wait_counter == 0)
                {
                    log<LOG_INFO>("Supervisor: Checking door.. Waiting.. %f") % wait_counter;
                }
                wait_counter++;
            }
            else
            {
                if (predoor_state == DEAD_END_FOUND)
                {
                    log<LOG_INFO>("Supervisor: Door gone or no door. Moving to LEAVE_DEAD_END.");
                    state = LEAVE_DEAD_END;
                    escape_dead_end = true;
                }
                else
                {
                    log<LOG_INFO>("Supervisor: Door gone or no door. Restored state and leaving area.");
                    state = predoor_state;
                    leaving_door = true;
                }
            }

            break;

        case LOOKING_FOR_SIDE:
            log<LOG_INFO>("Supervisor: Looking for junctions...");
            isGapRays = findGapRays(scan, 100);
            foundNode = node_recognizing(scan, rayToAngle, angleMargin);

            // DEAD_END case:
            if (foundNode == DEAD_END)
            {
                log<LOG_INFO>("Supervisor: Found node DEAD_END");
                if (escape_dead_end)
                {
                    log<LOG_INFO>("Supervisor: Not a door. Escaping dead end");
                }
                else
                {
                    log<LOG_INFO>("Supervisor: Potential door, moving to DEAD_END_FOUND");
                    escape_dead_end = false;
                    state = DEAD_END_FOUND;
                    predoor_state = state;
                }
            }
            // So if it's NOT a DEAD END
            else if (foundNode == OPEN_SPACE_LEFT || foundNode == OPEN_SPACE_RIGHT || foundNode == OPEN_SPACE_MIDDLE)
            {
                log<LOG_INFO>("Supervisor: Found an open space");
                escape_dead_end = false;
                // TODO: implement open space stuff here !!!
            } // NOR an OPEN_SPACE
            else if ((foundNode != STRAIGHT) && (foundNode != ERROR) && ((isGapRays[rightLaserBeam] == true) || (isGapRays[leftLaserBeam] == true)))
            {
                std::cout << "Supervisor: Found node: " << foundNode << "\n";
                escape_dead_end = false;
                // Run and store the pledge direction
                pledgeDirection = pledge(foundNode);

                // Start of open space case:
                // end of open space case
                if (pledgeDirection == M_STRAIGHT)
                {
                    state = ENTER_JUNCTION; // skip the turning states
                }
                else
                {
                    // Initialize midpoint here
                    midpointXY = midpoint_initialize(scan, pledgeDirection, foundNode);
                    state = MOVE_TO_MIDPOINT;
                }

            }
            break;
        case MOVE_TO_MIDPOINT:
            log<LOG_INFO>("Supervisor: Moving to junction center, preparing to turn");

            // Update the midpoint
            midpointXY = midpoint_tracking(scan, midpointXY);
            log<LOG_INFO>("\tLooking at: (%f, %f)" ) % midpointXY.getX() % midpointXY.getY();
            angle_rad = atan2(midpointXY.getY(), midpointXY.getX());
            angle_deg = -angle_rad * 180 / M_PI;
            //std::cout << "Supervisor: midpointX: " << midpointXY.getX() << "\n";
            //std::cout << "Supervisor: midpointY: " << midpointXY.getY() << "\n";
            if (pledgeDirection == LEFT)
            {
                if (midpointXY.getY() > 0)
                {
                    if (midpointXY.getX() < .1)
                    {
                        log<LOG_INFO>("Supervisor: Moved to midpoint (%f, %f), TURN_LEFT") % midpointXY.getX() % midpointXY.getY();
                        state = TURN_LEFT;
                    }
                }
                else
                {
                    log<LOG_INFO>("Supervisor: ERROR! Midpoint on wrong side!");
                    state = LOOKING_FOR_SIDE;
                }
            }
            else if (pledgeDirection == RIGHT)
            {
                if (midpointXY.getY() < 0)
                {
                    if (midpointXY.getX() < .1)
                    {
                        log<LOG_INFO>("Supervisor: Moved to midpoint (%f, %f), TURN_RIGHT") % midpointXY.getX() % midpointXY.getY();
                        state = TURN_RIGHT;
                    }
                }
                else
                {
                    log<LOG_INFO>("Supervisor: ERROR! Midpoint on wrong side!");
                    state = LOOKING_FOR_SIDE;
                }
            }
            else
            {
                log<LOG_INFO>("Supervisor: ERROR! Got move straight with move to Midpoint!");
                state = LOOKING_FOR_SIDE;
            }
            break;

        case U_TURN_LEFT:
            log<LOG_INFO>("Supervisor: U-turning left");
            midRays = findMidRays(scan);
            if (midRays.size() > 0)
            {
                angleToMidRay = rayToAngle[midRays[0]];
                //angle_deg = rayToAngle[midLaserBeam];
                angle_deg = 0;
                if ((angleToMidRay -  angle_deg < 3) || (abs(angleToMidRay) > 150) || (abs(angleToMidRay - angle_deg) > 150))
                {
                    log<LOG_INFO>("Supervisor: Uturn done, diff = %f, angleToMidray = %f, midLaserAngle = %f. Moving to LOOKING FOR SIDE") % (angleToMidRay - angle_deg) % angleToMidRay % angle_deg;
                    state = LOOKING_FOR_SIDE;
                }
                else
                {
                    log<LOG_INFO>("Supervisor: Angle %f !< 3. Keep U-turning") % (angleToMidRay - angle_deg);
                }
            }
            else
            {
                log<LOG_INFO>("Supervisor: No MidRays, keep U-turning");
            }
            break;

        case U_TURN_RIGHT:
            log<LOG_INFO>("Supervisor: U-Turning right");
            midRays = findMidRays(scan);
            if (midRays.size() > 0)
            {
                angleToMidRay = rayToAngle[midRays[0]];
                if ((angleToMidRay - angle_deg > -3) || (abs(angleToMidRay) > 150) || (abs(angleToMidRay - angle_deg) > 150))
                {
                    log<LOG_INFO>("Supervisor: Uturn done, diff = %f, angleToMidray = %f, midLaserAngle = %f. Moving to LOOKING FOR SIDE") % (angleToMidRay - angle_deg) % angleToMidRay % angle_deg;
                    state = LOOKING_FOR_SIDE;
                }
                else
                {
                    log<LOG_INFO>("Supervisor: Angle %f. Keep U-turning") % (angleToMidRay - angle_deg);
                }
            }
            else
            {
                log<LOG_INFO>("Supervisor: No MidRays, keep U-turning");
            }
            break;


        case TURN_LEFT:
            //TODO: Make turn complete detect more robust
            log<LOG_INFO>("Supervisor: turning left");
            //std::cout << "Supervisor: midpointX: " << midpointXY.getX() << "\n";
            //std::cout << "Supervisor: midpointY: " << midpointXY.getY() << "\n";

            // Update the midpoint
            midpointXY = midpoint_tracking(scan, midpointXY);
            midpointY = midpointXY.getY();
            angle_rad = atan2(midpointXY.getY(), midpointXY.getX());

            if((abs(angle_rad - midLaserBeam) <= RIGHT_LEFT_ANGLE_TOLERANCE) || (angle_rad < 0))
            {
                state = CROSS_JUNCTION;
                //log<LOG_INFO>("Supervisor: Done turning. Angle is %f rad") % angle_rad;
            }
            break;

        case TURN_RIGHT:
            //TODO: Make turn complete detect more robust
            log<LOG_INFO>("Supervisor: Turning right");
            //std::cout << "Supervisor: midpointX: " << midpointXY.getX() << "\n";
            //std::cout << "Supervisor: midpointY: " << midpointXY.getY() << "\n";

            // Update the midpoint
            midpointXY = midpoint_tracking(scan, midpointXY);
            midpointY = midpointXY.getY();

            angle_rad = atan2(midpointXY.getY(), midpointXY.getX());

            if((abs(angle_rad - midLaserBeam) <= RIGHT_LEFT_ANGLE_TOLERANCE) || (angle_rad > 0))
            {
                state = CROSS_JUNCTION;
                log<LOG_INFO>("Supervisor: Done turning. Angle is %f rad") % angle_rad;
            }
            break;

        case ENTER_JUNCTION:
            //TODO: Make enter junction detect more robust
            log<LOG_INFO>("Supervisor: Entering junction, going straight");
            isGapRays = findGapRays(scan, 100);

            // Added 30 to making sure that Pico is a bit deeper in the junction before it switches to the next state
            if(isGapRays[rightLaserBeam-30] == true || isGapRays[leftLaserBeam+30] == true)
            {
                log<LOG_INFO>("Supervisor: Entered junction");
                state = CROSS_JUNCTION;
            }
            break;
        case CROSS_JUNCTION:
            log<LOG_INFO>("Supervisor: Crossing junction");
            isGapRays = findGapRays(scan, 100);

            // Check if both sides are a wall
            //TODO: Make cross junction detect more robust
            if ((isGapRays[rightLaserBeam] == false) && (isGapRays[leftLaserBeam] == false))
            {
                log<LOG_INFO>("Supervisor: Out of junction, turn completed");
                state = LOOKING_FOR_SIDE;
            }
            break;
    }
    if (leaving_door)
    {
        if (!doorExist(scan) && !escape_dead_end)
            leaving_door = false;
    }
    return state;
}

std::vector<tf2::Vector3> targetxy;
tf2::Vector3 determine_target(emc::LaserData scan, state_t state)
{
    targetxy= findMidPoints(scan);
    tf2::Vector3 target;
    switch (state)
    {
        case LOOKING_FOR_SIDE:
            target.setValue(.1, 0, 0);
            break;
        case MOVE_TO_MIDPOINT:
            target.setValue(.1, 0, 0);
            break;
        case TURN_LEFT:
            target.setValue(0, 0, 0);
            break;
        case TURN_RIGHT:
            target.setValue(0, 0, 0);
            break;
        case U_TURN_LEFT:
            target.setValue(0, 0, 0);
            break;
        case U_TURN_RIGHT:
            target.setValue(0, 0, 0);
            break;
        case ENTER_JUNCTION:
            target.setValue(.1, 0, 0);
            break;
        case CROSS_JUNCTION:
            target.setValue(.1, 0, 0);
            break;
    }
    return target;
}