pledge

pledge snippet

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

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

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

int counter = 0;            // Initial: pledge counter
bool lastTurnRight = true;  // Initial: last time turned was to the right (random chosen). This is for the dead_end counter=0 case.
bool lastTurnU = false;     // Initial: last time it didn't make an U-turn
bool usePledge = true;     // True = pledge, False = random-walk

direction_t pledge(junction_t junction)
{
    direction_t direction = M_STRAIGHT;           // Initial state

    if (usePledge == true)
    {
        std::cout << "Pledge counter start: " << counter << "\n";
        switch (junction)
        {
            case DEAD_END:
                std::cout << "Pledge state: DEAD_END \n";
                if (counter == 0){
                    if (lastTurnRight == true) // Pico will follow the outer wall instead of turning back where he came from
                    {
                        direction = LEFT;
                        counter = counter - 2;
                    }
                    else
                    {
                        direction = RIGHT;
                        counter = counter + 2;
                    }
                }
                else if (counter < 0){
                    if (lastTurnU == true){ // Turn the opposite direction to prevent being stuck in two dead-ends
                        direction = LEFT;
                        counter = counter - 2;}
                    else{
                        direction = RIGHT;
                        counter = counter + 2;}
                    }
                else{
                    if (lastTurnU == true){ // Turn the opposite direction to prevent being stuck in two dead-ends
                        direction = RIGHT;
                        counter = counter + 2;}
                    else{
                        direction = LEFT;
                        counter = counter - 2;}
                }
                lastTurnU = true;
                break;
            case CROSS:
                if (counter == 0){
                    direction = M_STRAIGHT;}
                else if (counter < 0){
                    direction = RIGHT;
                    lastTurnU = false;
                    ++counter;}
                else{
                    direction = LEFT;
                    lastTurnU = false;
                    --counter;}
                break;
            case T_JUNCTION_RL:
                if (counter == 0){
                    direction = RIGHT;
                    ++counter;}
                else if (counter < 0){
                    direction = RIGHT;
                    ++counter;}
                else{
                    direction = LEFT;
                    --counter;}
                lastTurnU = false;
                break;
            case T_JUNCTION_FL:
                if (counter == 0){
                    direction = M_STRAIGHT;}
                else if (counter < 0){
                    direction = M_STRAIGHT;}
                else{
                    direction = LEFT;
                    lastTurnU = false;
                    --counter;}
                break;
            case T_JUNCTION_RF:
                if (counter == 0){
                    direction = M_STRAIGHT;}
                else if (counter < 0){
                    direction = RIGHT;
                    lastTurnU = false;
                    ++counter;}
                else{
                    direction = M_STRAIGHT;}
                break;
            case CORNER_LEFT:
                std::cout << "Pledge state: left \n";
                direction = LEFT;
                --counter;
                lastTurnU = false;
                break;
            case CORNER_RIGHT:
               std::cout << "Pledge state: right \n";
                direction = RIGHT;
                ++counter;
                lastTurnU = false;
                break;
        }

    // Update lastTurnRight for the next time pledge is called
        if (direction == LEFT)
        {
            lastTurnRight = false;
        }
        else if (direction == RIGHT)
        {
             lastTurnRight = true;
        }
        std::cout << "Pledge counter end: " << counter << "\n";
        std::cout << "Pledge direction: " << direction << "\n";
    } //(usePledge == true)
    else //random walk
    {
        srand(time(0));
        int flag = (rand() % 2)+1;
        switch (junction)
        {
            case DEAD_END:
            {   std::cout << "random-walk state: DEAD_END \n";
                direction = RIGHT;
                break;
            }
            case CROSS:
            {   //int dice1 = (rand() % 2)+1;
                std::cout<<"cross flag="<<flag;
                if (flag == 1){
                    direction = M_STRAIGHT;}
                else if (flag  == 2){
                    direction = RIGHT;
                    }
                else if (flag ==3){
                    direction = LEFT;
                    }
                break;
            }
            case T_JUNCTION_RL:
            {   //int dice4 = (rand() % 1)+1;
                std::cout<<"c_JUNCTION_RL flag="<<(flag-1);
                if ((flag-1)== 1){
                    direction = RIGHT;
                    }
                else if ((flag-1)==0){
                    direction = LEFT;
                    }
                break;
            }
            case T_JUNCTION_FL:
            {   //int dice2 = (rand() % 1)+1;
                std::cout<<"T_JUNCTION_FL flag="<<(flag-1);
                if ((flag-1) == 1){
                    direction = M_STRAIGHT;}
                else if ((flag-1)==0){
                    direction = LEFT;
                    }
                break;
            }
            case T_JUNCTION_RF:
            {   //int dice3 = (rand() % 1)+1;
                std::cout<<"T_JUNCTION_RF flag="<<(flag-1);
                if ((flag-1) == 1){
                    direction = M_STRAIGHT;}
                else if ((flag-1)==0){
                    direction = RIGHT;}
                break;
            }
            case CORNER_LEFT:
            {
                direction = LEFT;
                break;
            }
            case CORNER_RIGHT:
            {
                direction = RIGHT;
                break;
            }
        }
        std::cout << "Random-walk: direction: " << direction << "\n";
    }

    return direction;
}