Colony » Colony Scout » Colony Scout OS

#include "navigationMap.h"

using namespace std;

/** @brief Initializes the map */

navigationMap::navigationMap(string scoutname) : Behavior(scoutname, "navigationMap")

{

  /** Initialize Map 

   *

   *   1           2          3         4

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

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

   *      |           |          |         |

   *     9|         10|        11|       12|

   *      |           |          |         |

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

   */

    Edge* a1 = new Edge[ARRAY_SIZE];

    a1[0] = MAKE_EDGE(ISTRAIGHT, 2, 10);

    a1[1] = MAKE_EDGE(IRIGHT, 9, 40);

    a1[2] = MAKE_EDGE(IUTURN, DEADEND, 0);

    Edge* a2 = new Edge[ARRAY_SIZE]; 

    a2[0] = MAKE_EDGE(ISTRAIGHT, 3, 10);

    a2[1] = MAKE_EDGE(IRIGHT, 10, 40);

    a2[2] = MAKE_EDGE(IUTURN, 5, 10);

    Edge* a3 = new Edge[ARRAY_SIZE]; 

    a3[0] = MAKE_EDGE(ISTRAIGHT, 4, 10);

    a3[1] = MAKE_EDGE(IRIGHT, 11, 40);

    a3[2] = MAKE_EDGE(IUTURN, 6, 10);

    Edge* a4 = new Edge[ARRAY_SIZE]; 

    a4[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0);

    a4[1] = MAKE_EDGE(IRIGHT, 12, 40);

    a4[2] = MAKE_EDGE(IUTURN, 7, 10);

    Edge* a5 = new Edge[ARRAY_SIZE];

    a5[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0);

    a5[1] = MAKE_EDGE(ILEFT, 9, 40);

    a5[2] = MAKE_EDGE(IUTURN, 2, 10);

    Edge* a6 = new Edge[ARRAY_SIZE];

    a6[0] = MAKE_EDGE(ISTRAIGHT, 5, 0);

    a6[1] = MAKE_EDGE(ILEFT, 10, 40);

    a6[2] = MAKE_EDGE(IUTURN, 3, 10);

    Edge* a7 = new Edge[ARRAY_SIZE];

    a7[0] = MAKE_EDGE(ISTRAIGHT, 6, 0);

    a7[1] = MAKE_EDGE(ILEFT, 11, 40);

    a7[2] = MAKE_EDGE(IUTURN, 3, 10);

    Edge* a8 = new Edge[ARRAY_SIZE];

    a8[0] = MAKE_EDGE(ISTRAIGHT, 7, 0);

    a8[1] = MAKE_EDGE(ILEFT, 12, 40);

    a8[2] = MAKE_EDGE(IUTURN, DEADEND, 10);

    Edge* a9 = new Edge[ARRAY_SIZE];

    a9[0] = MAKE_EDGE(IRIGHT, 2, 10);

    a9[1] = MAKE_EDGE(ILEFT, DEADEND, 0);

    a9[2] = MAKE_EDGE(IUTURN, 9, 40);

    Edge* a10 = new Edge[ARRAY_SIZE];

    a10[0] = MAKE_EDGE(IRIGHT, 3, 10);

    a10[1] = MAKE_EDGE(ILEFT, 5, 10);

    a10[2] = MAKE_EDGE(IUTURN, 10, 40);

    Edge* a11 = new Edge[ARRAY_SIZE];

    a11[0] = MAKE_EDGE(IRIGHT, 4, 10);

    a11[1] = MAKE_EDGE(ILEFT, 6, 10);

    a11[2] = MAKE_EDGE(IUTURN, 11, 40);

    Edge* a12 = new Edge[ARRAY_SIZE];

    a12[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);

    a12[1] = MAKE_EDGE(ILEFT, 7, 10);

    a12[2] = MAKE_EDGE(IUTURN, 12, 40);

    map.push_back(a1);

    map.push_back(a2);

    map.push_back(a3);

    map.push_back(a4);

    map.push_back(a5);

    map.push_back(a6);

    map.push_back(a7);

    map.push_back(a8);

    map.push_back(a9);

    map.push_back(a10);

    map.push_back(a11);

    map.push_back(a12);

    curr_state = START_STATE;

    arrival_time = ros::TIME_MAX;

}

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

navigationMap::~navigationMap()

{

  while(!map.empty())

  {

    Edge* temp = map.back();

    map.pop_back();

    delete temp;

  }

  return;

}

void navigationMap::run()

{

  Duration t;

  ROS_INFO("My state is: %d\n", curr_state);

  //Straight, straight, right, left, straight 5

  update_state(ISTRAIGHT);

  ROS_INFO("My state is: %d\n", curr_state);

  t = get_time_remaining();

  while(t.toSec() > 0)

    t = get_time_remaining();

  update_state(ISTRAIGHT);

  ROS_INFO("My state is: %d\n", curr_state);

  t = get_time_remaining();

  while(t.toSec() > 0)

    t = get_time_remaining();

  update_state(IRIGHT);

  ROS_INFO("My state is: %d\n", curr_state);

  t = get_time_remaining();

  while(t.toSec() > 0)

    t = get_time_remaining();

  update_state(ILEFT);

  ROS_INFO("My state is: %d\n", curr_state);

  t = get_time_remaining();

  while(t.toSec() > 0)

    t = get_time_remaining();

  update_state(ISTRAIGHT);

  ROS_INFO("My state is: %d\n", curr_state);

  t = get_time_remaining();

  while(t.toSec() > 0)

    t = get_time_remaining();

  ROS_INFO("Traveled route!\n");

  while(ok())

    continue;

}

State navigationMap::update_state(Turn turn_made)

{

  Edge* possible_edges = get_outbound_edges(curr_state);

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

  for(int 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

      curr_state = GET_EDGE_STATE(possible_edges[i]);

      //TODO: get actual speed

      Duration travel_time(GET_EDGE_DIST(possible_edges[i])/speed);

      arrival_time = Time::now() + travel_time;

      return curr_state;

    }

  }

  return -1;//failure to succeed

}

Time navigationMap::get_eta()

{

  return arrival_time;

}

Duration navigationMap::get_time_remaining()

{

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

}

State navigationMap::get_state()

{

  return curr_state;

}

Edge* navigationMap::get_outbound_edges(State state)

{

  return map.at(state-1); 

}

Path navigationMap::shortest_path(State target_state)

{

  // BFS algorithm

  State curr_state = get_state();

  int visited[MAX_NODES+1] = {0};

  queue<State> q;

  q.push(curr_state);

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

  visited[curr_state] = -1;

  while (!q.empty())

  {

    State state = q.front();

    //actually dequeue it

    q.pop();

    if (state == target_state)

    {   

      Path path;

      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)

          {

            path.path[j] = GET_EDGE_DIR(edges[i]);

            j++;

            break;

          }

        }

      }

      /** Reverse moves list */

      for (int i = 0; i < j/2; i++)

      {

        path.path[i] ^= path.path[j-i-1];

        path.path[j-i-1] ^= path.path[i];

        path.path[i] ^= path.path[j-i-1];

      }

      path.len = j;

      return path;

    }

    Edge* edges = get_outbound_edges(state);

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

    {

      State new_state = GET_EDGE_STATE(edges[i]);

      if (new_state != DEADEND && !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

  Path path;

  path.len = 0;

  path.path = NULL;

  return path;

}