Colony » Colony Scout

#include "navigationMap.h"

/** @brief Initializes the map */

navigationMap::navigationMap(std::string scoutname) : Behavior(scoutname)

{

  /** Initialize Map 

   *

   *   1           2          3         4

   *  ----|-----------|----------|---------|---------->

   *  <---|--5--------|--6-------|--7------|--8-------

   *      |           |          |         |

   *     9|         10|        11|       12|

   *      |           |          |         |

   *     ---         ---        ---       ---

   */

    Edge a1[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 2, 10), 

                            MAKE_EDGE(IRIGHT, 9, 40),

                            MAKE_EDGE(IUTURN, DEADEND, 0)};

    Edge a2[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 3, 10),

                            MAKE_EDGE(IRIGHT, 10, 40),

                            MAKE_EDGE(IUTURN, 5, 10)};

    Edge a3[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 4, 10),

                            MAKE_EDGE(IRIGHT, 11, 40),

                            MAKE_EDGE(IUTURN, 6, 10)};

    Edge a4[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),

                            MAKE_EDGE(IRIGHT, 12, 40),

                            MAKE_EDGE(IUTURN, 7, 10)};

    Edge a5[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),

                            MAKE_EDGE(ILEFT, 9, 40),

                            MAKE_EDGE(IUTURN, 2, 10)};

    Edge a6[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 5, 10),

                            MAKE_EDGE(ILEFT, 10, 40),

                            MAKE_EDGE(IUTURN, 3, 10)};

    Edge a7[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 6, 10),

                            MAKE_EDGE(ILEFT, 11, 40),

                            MAKE_EDGE(IUTURN, 4, 10)};

    Edge a8[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 7, 10),

                            MAKE_EDGE(ILEFT, 12, 40),

                            MAKE_EDGE(IUTURN, DEADEND, 0)};

    Edge a9[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 2, 10),

                            MAKE_EDGE(ILEFT, DEADEND, 0),

                            MAKE_EDGE(IUTURN, 9, 40)};

    Edge a10[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 3, 10),

                            MAKE_EDGE(ILEFT, 5, 10),

                            MAKE_EDGE(IUTURN, 10, 40)};

    Edge a11[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 4, 10),

                            MAKE_EDGE(ILEFT, 6, 10),

                            MAKE_EDGE(IUTURN, 11, 40)};

    Edge a12[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, DEADEND, 0),

                            MAKE_EDGE(ILEFT, 7, 10),

                            MAKE_EDGE(IUTURN, 12, 40)};

    map.pushback(a1);

    map.pushback(a2);

    map.pushback(a3);

    map.pushback(a4);

    map.pushback(a5);

    map.pushback(a6);

    map.pushback(a7);

    map.pushback(a8);

    map.pushback(a9);

    map.pushback(a10);

    map.pushback(a11);

    map.pushback(a12);

        curr_state = START_STATE;

    eta = 99999;

}

/** @brief Goes through and frees all allocated memory */

navigationMap::~navigationMap()

{

    while(!map.empty())

    {

      Edge* e = map.pop_back();

      delete e;

    }

    return;

}

navigationMap::run()

{

}

navigationMap::update_state(Turn turn_made)

{

        Edge* possible_edges = get_outbound_edges(current_state);

        int arr_size = sizeof(possible_edges)/sizeof(Edge);

        for(i=0;i<arr_size;i++)

        {

                //sets the current state to the state associated with the turn made

                if(GET_EDGE_DIR(possible_edges[i]) == turn_made)

                {

                        int speed = 10000;//its over 9000

                        current_state = GET_EDGE_STATE(possible_edges[i]);

                        //TODO: get actual speed

                        arrival_time = ros::Time::now() + 

                                        GET_EDGE_DIST(possible_edges[i])/speed;

                        return current_state;

                }

        }

        return -1;//failure to succeed

}

navigationMap::get_eta()

{

        return arrival_time;

}

navigationMap::get_time_remaining()

{

        return (arrival_time - ros::Time::now());

}

navigationMap::get_state()

{

        return curr_state;

}

navigationMap::get_outbound_edges(State state)

{

        return map(state-1); 

}

navigationMap::shortest_path(State target_state)

{

    // BFS algorithm

    State curr_state = get_state();

    char visited[MAX_NODES+1]; 

    queue<State> q;

    q.push(curr_state);

    // not zero = visited, zero = unvisited, negative = start state

    visited[curr_state] = -1;

    while (!q.is_empty)

    {

        State state = q.pop();

        if (state == target_state)

        {   

            Turn moves_list[MAX_NODES];

            int j = 0; // counter

            for(State child = state; visited[child] >= 0; 

                    child = visited[child]) //while not start state

            {

                State parent = visited[child];

                Edge* edges = get_outbound_edges(parent);

                for (int i = 0; i < ARRAY_SIZE; i++)

                {

                    if (GET_EDGE_STATE(edges[i]) == child)

                    {

                        moves_list[j] = GET_EDGE_DIR(edges[i]);

                        j++;

                        break;

                    }

                }

            }

            return moves_list;

        }

        Edge* edges = get_outbound_edges(state);

        int length = sizeof(edges) / sizeof(Edge);

        for (int i = 0; i < length; i++)

        {

            State new_state = GET_EDGE_STATE(edges[i]);

            if (!visited[new_state]) 

            {

                // set visited to the parent of the new state

                visited[new_state] = state;

                q.push(new_state);

            }

        }

    }

    //oops, no way to get to target from state

    return NULL;

}