Colony » Colony Scout » Colony Scout OS

#ifndef _NAVIGATION_MAP_

#define _NAVIGATION_MAP_

#include <cstdlib>

#include <queue>

#include "lineDrive.h" // Get turn Macros

#define START_STATE 1

#define DEADEND -1

#define ARRAY_SIZE 3

#define MAX_NODES 12

#define GET_EDGE_DIR(edge) ((edge)&0x3)

#define GET_EDGE_STATE(edge) (((edge)>>2)&0xFF)

#define GET_EDGE_DIST(edge) (((edge)>>10)&0x3FFFFF)

#define SET_EDGE_DIR(dir) ((dir)&0x3)

#define SET_EDGE_STATE(state) (((state)&0xFF)<<2)

#define SET_EDGE_DIST(dist) (((dist)&0x3FFFFF)<<10)

#define MAKE_EDGE(dir, state, dist) \

  SET_EDGE_DIR(dir)+SET_EDGE_STATE(state)+SET_EDGE_DIST(dist)

typedef int Edge;

typedef int State;

typedef int Turn;

typedef ros::Time Time;

typedef struct{

  int len;

  Turn* path;

} Path;

class navigationMap : Behavior

{

  public:

    navigationMap(std::string scoutname);

    ~navigationMap();

    void run();

    State update_state(Turn turn_made);

    Time get_eta();

    Time get_time_remaining();

    State get_state();

    Path shortest_path(State target_state);

    Edge* get_outbound_edges(State state);        

  private:

    vector <Edge*> map;

    State curr_state;

    Time arrival_time;

};

#endif