Added sensor rendering.

ant.world

@@ -35,7 +35,7 @@ window
 
 floorplan
 (
-	bitmap "maps/empty.png"
+	bitmap "maps/cave.png"
 	size [ 16.000 16.000 1.500 ]
 )
 

gui.cpp

@@ -28,12 +28,14 @@
 #include "gui.hpp"
 #include "ogl.hpp"
 
-static void redraw_callback(void * arg) {
+namespace gui {
+
+  static void redraw_callback(void * arg) {
     GlGui * window = static_cast<GlGui *>(arg);
     window->redraw();
-}
+  }
 
-GlGui::GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, PheromoneMap * phero_map) : Fl_Gl_Window(x, y, w, h, l) {
+  GlGui::GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, ias_ss::PheromoneMap * phero_map) : Fl_Gl_Window(x, y, w, h, l) {
     mode(FL_RGB | FL_DOUBLE);
 
     this->parent = parent;
@@ -42,9 +44,9 @@ GlGui::GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, Phe
     initialized = false;
 
     Fl::add_idle(redraw_callback, this);
-}
+  }
 
-void GlGui::draw() {
+  void GlGui::draw() {
     if(!valid()) {
       if(!initialized) {
 	ogl::initialize(phero_map);
@@ -55,8 +57,22 @@ void GlGui::draw() {
 
     phero_map->s_evaporate();
     ogl::display();
-}
+  }
 
-int GlGui::handle(int event) {
+  int GlGui::handle(int event) {
     return Fl_Gl_Window::handle(event);
+  }
+
+  void GlSensorGui::draw() {
+    if(!valid()) {
+      if(!initialized) {
+	ogl::initialize(phero_map);
+	initialized = true;
+      }
+      ogl::reshape(w(), h());
+    }
+
+    ogl::display_sensor_map();
+  }
+
 }

gui.hpp

@@ -34,19 +34,27 @@
 
 #include "pheromone.hpp"
 
-class GlGui : public Fl_Gl_Window {
+namespace gui {
-public:
+  class GlGui : public Fl_Gl_Window {
-  GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, PheromoneMap * phero_map);
+  public:
+    GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, ias_ss::PheromoneMap * phero_map);
 
-protected:
+  protected:
     virtual void draw();
     virtual int handle(int);
-
-private:
     Fl_Window * parent;
     std::string title;
     bool initialized;
-  PheromoneMap * phero_map;
+    ias_ss::PheromoneMap * phero_map;
-};
+  };
+
+  class GlSensorGui : public GlGui {
+  public:
+    GlSensorGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, ias_ss::PheromoneMap * phero_map): GlGui(parent, x, y, w, h, l, phero_map) { };
+
+  protected:
+    virtual void draw();
+  };
+}
   
 #endif

ias_robot.cpp

@@ -36,34 +36,35 @@
 
 //#define PROB_MODEL_1
 
-static const float  TURN_DEG_PER_SEC = 90.0f;
+namespace ias_ss {
-static const float  METERS_PER_SEC   = 0.4f;
+  static const float  TURN_DEG_PER_SEC = 90.0f;
-static const long   HALF_SECOND_USEC = 500000;
+  static const float  METERS_PER_SEC   = 0.4f;
-static const double MIN_DIST_M       = 1.0;
+  static const long   HALF_SECOND_USEC = 500000;
-static const double CRIT_DIST_M      = 0.7;
+  static const double MIN_DIST_M       = 1.0;
-static const float  MAP_SIZE         = 16.0f;
+  static const double CRIT_DIST_M      = 0.7;
-static const int    PHERO_AMOUNT     = 30;
+  static const float  MAP_SIZE         = 16.0f;
-static const float  PHERO_RADIUS     = 1.4f;
+  static const int    PHERO_AMOUNT     = 30;
-static const float  SENSOR_RADIUS    = 2.0f;
+  static const float  PHERO_RADIUS     = 1.4f;
-static const float  U_RATIO          = 0.3f;
+  static const float  SENSOR_RADIUS    = 2.0f;
-static const float  V_RATIO          = 0.1f;
+  static const float  U_RATIO          = 0.3f;
-static const float  SMOOTHING_COEFF  = 0.3f;
+  static const float  V_RATIO          = 0.1f;
+  static const float  SMOOTHING_COEFF  = 0.3f;
 
-static inline float random_num() {
+  static inline float random_num() {
     return (((static_cast<float>(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * PHERO_RADIUS;
-}
+  }
 
-IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port, PheromoneMap * phero_map) : Robot(hostname, port) {
+  IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port, PheromoneMap * phero_map) : Robot(hostname, port) {
     srand(port + time(NULL));
     _phero_map = phero_map;
     log("Creating IAS-SS robot");
-}
+  }
 
-IASSS_Robot::~IASSS_Robot() {
+  IASSS_Robot::~IASSS_Robot() {
     log("Destroying IAS-SS robot");
-}
+  }
 
-void IASSS_Robot::run() {
+  void IASSS_Robot::run() {
     float x, y, steer = 0.0f;
     int    rv;
     long   then, now, delta, wait;
@@ -107,9 +108,9 @@ void IASSS_Robot::run() {
     // Sleep for a bit before finishing this control iteration.
     wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC;
     usleep(wait);
-}
+  }
 
-void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) {
+  void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) {
     double dist_l = 0.0;
     double dist_r = 0.0;
 
@@ -125,14 +126,14 @@ void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) {
       _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(-turn_speed));
     else
       _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(turn_speed));
-}
+  }
 
-void IASSS_Robot::deposit_pheromone(float x, float y) {
+  void IASSS_Robot::deposit_pheromone(float x, float y) {
     for(int i = 0; i < PHERO_AMOUNT; i++)
       _phero_map->s_deposit_pheromone(x, y, _p_proxy->GetYaw(), PHERO_RADIUS / MAP_SIZE);
-}
+  }
 
-float IASSS_Robot::brss() {
+  float IASSS_Robot::brss() {
     std::map<int, float> U, V;
     unsigned int         i_min, i_max;
     float                min, sample, prob, max, sum_uv = 0.0f, steer;
@@ -235,4 +236,5 @@ float IASSS_Robot::brss() {
 #endif
 
     return steer;
+  }
 }

ias_robot.hpp

@@ -29,7 +29,8 @@
 #include "robot.hpp"
 #include "pheromone.hpp"
 
-/**
+namespace ias_ss {
+  /**
    * Concrete robot that implements the IAS-SS architecture as defined in:
    *
    *   1) R. Calvo et al. "Inverse ACO for Exploration and Surveillance in
@@ -40,19 +41,20 @@
    *      for Multiple Agent Systems Applied to Surveillance Tasks in Unknown
    *      Environments", Proc. of the IEEE IJCNN, San Jose, USA, 2011.
    */
-class IASSS_Robot : Robot {
+  class IASSS_Robot : Robot {
-public:
+  public:
     IASSS_Robot(std::string hostname, uint32_t port, PheromoneMap * phero_map);
     virtual ~IASSS_Robot();
     virtual void run();
 
-private:
+  private:
     PheromoneMap * _phero_map;
     phero_sensor_t _phero_sensor;
   
     void avoid_wall(float front_speed, float turn_speed);
     void deposit_pheromone(float x, float y);
     float brss();
-};
+  };
+}
 
 #endif

ias_ss.cpp

@@ -32,6 +32,7 @@
 #include "ias_robot.hpp"
 
 const char * TITLE = "Pheromone map";
+const char * S_TITLE = "Pheromone sensor";
 const int W = 512;
 const int H = 512;
 const uint32_t PORT = PlayerCc::PLAYER_PORTNUM + 1;
@@ -39,15 +40,17 @@ const uint32_t NUM_ROBOTS = 4;
 
 static bool                done      = false;
 static Fl_Window *         window    = NULL;
-static GlGui *        glWindow  = NULL;
+static Fl_Window *         sWindow    = NULL;
-static PheromoneMap * phero_map = NULL;
+static gui::GlGui *        glWindow  = NULL;
+static gui::GlSensorGui *  glSWindow = NULL;
+static ias_ss::PheromoneMap *      phero_map = NULL;
 
 extern "C" void handler(int signal) {
   done = true;
 }
   
 extern "C" void * robot_thread(void * arg) {
-  IASSS_Robot * robot = static_cast<IASSS_Robot *>(arg);
+  ias_ss::IASSS_Robot * robot = static_cast<ias_ss::IASSS_Robot *>(arg);
 
   std::cout << "Running robot thread." << std::endl;
 
@@ -61,24 +64,30 @@ extern "C" void * robot_thread(void * arg) {
 
 void create_gui(int argc, char **argv) {
   window = new Fl_Window(20, 40, W, H, TITLE);
-  glWindow = new GlGui(window, 0, 0, W, H, TITLE, phero_map);
+  glWindow = new gui::GlGui(window, 0, 0, W, H, TITLE, phero_map);
   window->end();
   window->show(argc, argv);
   window->make_current();
+
+  sWindow = new Fl_Window(20, 40, W, H, S_TITLE);
+  glSWindow = new gui::GlSensorGui(sWindow, 0, 0, W, H, S_TITLE, phero_map);
+  sWindow->end();
+  sWindow->show(argc, argv);
+  sWindow->make_current();
 }
 
 int main(int argc, char **argv) {
   pthread_t                  robot_threads[NUM_ROBOTS];
-  std::vector<IASSS_Robot *> robots;
+  std::vector<ias_ss::IASSS_Robot *> robots;
 
   signal(SIGINT, handler);
 
   try {
-    phero_map = new PheromoneMap(argc > 1 ? argv[1] : "maps/cave_mask.png");
+    phero_map = new ias_ss::PheromoneMap(argc > 1 ? argv[1] : "maps/cave_mask.png");
 
     // Initialize the robot objects and threads.
     for(uint32_t i = 0; i < NUM_ROBOTS; ++i) {
-      robots.push_back(new IASSS_Robot(PlayerCc::PLAYER_HOSTNAME, PORT + i, phero_map));
+      robots.push_back(new ias_ss::IASSS_Robot(PlayerCc::PLAYER_HOSTNAME, PORT + i, phero_map));
 
       if(pthread_create(&robot_threads[i], NULL, robot_thread, static_cast<void *>(robots[i])) != 0) {
 	perror("Could not create robot thread");

ogl.cpp

@@ -37,15 +37,16 @@ namespace ogl
 { 
   // Variables
   static CGLSLProgram m_program;
-  static PheromoneMap * m_phero_map = NULL;
+  static ias_ss::PheromoneMap * m_phero_map = NULL;
   static GLuint m_textureHandle;
+  static GLuint m_sensorTextureHandle;
   static GLuint m_colorMapHandle;
 
   // Quad definition
   static glm::vec4 vec_points[6];
   static glm::vec2 vec_tex_coords[6];
   
-  static void quad() {
+  static inline void quad() {
     vec_tex_coords[0] = glm::vec2( 0.0f,  1.0f); vec_points[0] = glm::vec4( -0.5f, -0.5f, 0.0f, 1.0f );
     vec_tex_coords[1] = glm::vec2( 0.0f,  0.0f); vec_points[1] = glm::vec4( -0.5f,  0.5f, 0.0f, 1.0f );
     vec_tex_coords[2] = glm::vec2( 1.0f,  1.0f); vec_points[2] = glm::vec4(  0.5f, -0.5f, 0.0f, 1.0f );
@@ -76,7 +77,7 @@ namespace ogl
     delete data;
   }
 
-  void initialize(PheromoneMap * phero_map) {
+  void initialize(ias_ss::PheromoneMap * phero_map) {
     glEnable(GL_TEXTURE_1D);
     glEnable(GL_TEXTURE_2D);
 
@@ -130,6 +131,27 @@ namespace ogl
     m_program.disable();
   }
 
+  void display_sensor_map() {
+    glClear(GL_COLOR_BUFFER_BIT);
+    glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
+
+    if(m_phero_map != NULL)
+      m_sensorTextureHandle = m_phero_map->s_build_sensor_texture();
+
+    glActiveTexture(GL_TEXTURE0);
+    glBindTexture(GL_TEXTURE_2D, m_sensorTextureHandle);
+    
+    glBegin(GL_TRIANGLES); {
+      for(int i = 0; i < 6; i++) {
+	glTexCoord2f(vec_tex_coords[i].s, vec_tex_coords[i].t);
+	glVertex4f(vec_points[i].x, vec_points[i].y, vec_points[i].z, vec_points[i].w);
+      }
+    } glEnd();
+
+    glActiveTexture(GL_TEXTURE0);
+    glBindTexture(GL_TEXTURE_2D, 0);
+  }
+  
   void reshape(int w, int h) {
     if(h == 0)
       h = 1;

ogl.hpp

@@ -29,8 +29,9 @@
 #include "pheromone.hpp"
 
 namespace ogl {
-  void initialize(PheromoneMap * phero_map);
+  void initialize(ias_ss::PheromoneMap * phero_map);
   void display();
+  void display_sensor_map();
   void reshape(int w, int h);
 }
 

pheromone.cpp

@@ -27,54 +27,60 @@
 #include <cstdlib>
 #include <cmath>
 #include <cassert>
+#include <cstring>
+
 #include <pnglite.h>
 #include <glm/glm.hpp>
 #include <libplayerc++/playerc++.h>
 
 #include "pheromone.hpp"
 
-#define MAP_POS(X, Y) (data[((X) * m_height) + (Y)])
+#define MAP_POS(X, Y, Z) (Z[((X) * m_height) + (Y)])
 
-static const unsigned int  MAX_ITERS           = 1000;
+namespace ias_ss {
-static const float         EVAPORATION_RATE    = 0.05f;
+  static const unsigned int  MAX_ITERS           = 1000;
-const        unsigned char MAX_PHERO_INTENSITY = 250;
+  static const float         EVAPORATION_RATE    = 0.05f;
-const        unsigned char MIN_PHERO_INTENSITY = 0;
+  const        unsigned char MAX_PHERO_INTENSITY = 250;
+  const        unsigned char MIN_PHERO_INTENSITY = 0;
 
-static inline int sign(float f) {
+  static inline int sign(float f) {
     return (f < 0.0f) ? -1 : ((f > 0.0f) ? 1 : 0);
-}
+  }
 
-static inline int side(glm::vec3 line, glm::vec3 v) {
+  static inline int side(glm::vec3 line, glm::vec3 v) {
     return sign(glm::cross(line, v).z);
-}
+  }
 
-static inline float random_n(float r) {
+  static inline float random_n(float r) {
     return (((static_cast<float>(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * r;
-}
+  }
 
-PheromoneMap::PheromoneMap(const char * file_name) {
+  PheromoneMap::PheromoneMap(const char * file_name) {
     load_map(file_name);
     sem_init(&map_semaphore, 0, 1);
     then = 0;
-
+    sensor_updates = 0;
     glGenTextures(1, &handle);
-}
+    glGenTextures(1, &sensor_handle);
+  }
 
-PheromoneMap::~PheromoneMap() {
+  PheromoneMap::~PheromoneMap() {
     delete data;
     sem_destroy(&map_semaphore);
 
     glActiveTexture(GL_TEXTURE0);
     glBindTexture(GL_TEXTURE_2D, handle);
     glDeleteTextures(1, &handle);
-}
+    glDeleteTextures(1, &sensor_handle);
+  }
 
-void PheromoneMap::load_map(const char * file_name) {
+  void PheromoneMap::load_map(const char * file_name) {
     png_t tex;
 
     png_init(0, 0);
     png_open_file_read(&tex, file_name);
     data = new unsigned char[tex.width * tex.height * tex.bpp];
+    sensor_data = new unsigned char[tex.width * tex.height * tex.bpp];
     png_get_data(&tex, data);
 
     std::cout << "Loaded map \"" << file_name << "\" :: " << tex.width << "x" << tex.height << "x" << (int)tex.bpp << std::endl;
@@ -83,9 +89,9 @@ void PheromoneMap::load_map(const char * file_name) {
     m_bpp    = tex.bpp;
   
     png_close_file(&tex);
-}
+  }
 
-GLuint PheromoneMap::s_build_texture() {
+  GLuint PheromoneMap::s_build_texture() {
     glActiveTexture(GL_TEXTURE0);
     glBindTexture(GL_TEXTURE_2D, handle);
     sem_wait(&map_semaphore); {
@@ -98,10 +104,26 @@ GLuint PheromoneMap::s_build_texture() {
     glBindTexture(GL_TEXTURE_2D, 0);
   
     return handle;
-}
+  }
 
-void PheromoneMap::s_deposit_pheromone(float x, float y, float yaw, float radius) {
+  GLuint PheromoneMap::s_build_sensor_texture() {
-  unsigned int iters = 0, _x, _y;
+    glActiveTexture(GL_TEXTURE0);
+    glBindTexture(GL_TEXTURE_2D, sensor_handle);
+    sem_wait(&map_semaphore); {
+      glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, m_width, m_height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, sensor_data);
+    } sem_post(&map_semaphore);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+
+    glActiveTexture(GL_TEXTURE0);
+    glBindTexture(GL_TEXTURE_2D, 0);
+  
+    return sensor_handle;
+  }
+
+  void PheromoneMap::s_deposit_pheromone(float x, float y, float yaw, float radius) {
+    unsigned int iters = 0;
+    int _x, _y;
     bool         valid = false;
     float        dist, cos_theta, r_x, r_y;
     glm::vec2    v, vp;
@@ -127,48 +149,48 @@ void PheromoneMap::s_deposit_pheromone(float x, float y, float yaw, float radius
       _x = _x < 0 ? _x = 0 : (_x >= m_width ? m_width - 1 : _x);
       _y = _y < 0 ? _y = 0 : (_y >= m_height ? m_height - 1 : _y);
       sem_wait(&map_semaphore); {
-      if(MAP_POS(_y, _x) <= MAX_PHERO_INTENSITY) {
+	if (MAP_POS(_y, _x, data) <= MAX_PHERO_INTENSITY) {
-	MAP_POS(_y, _x) += rand() % MAX_PHERO_INTENSITY;
+	  MAP_POS(_y, _x, data) += rand() % MAX_PHERO_INTENSITY;
-	MAP_POS(_y, _x) = MAP_POS(_y, _x) > MAX_PHERO_INTENSITY ? MAX_PHERO_INTENSITY : MAP_POS(_y, _x);
+	  MAP_POS(_y, _x, data) = MAP_POS(_y, _x, data) > MAX_PHERO_INTENSITY ? MAX_PHERO_INTENSITY : MAP_POS(_y, _x, data);
 	  valid = true;
 	}
       } sem_post(&map_semaphore);
       iters++;
-    if(iters > MAX_ITERS)
+      if (iters > MAX_ITERS)
 	break;
     } while(!valid);
-}
+  }
 
-void PheromoneMap::s_evaporate() {
+  void PheromoneMap::s_evaporate() {
     unsigned char p_eva;
 
     clock_t now = clock();
-  if(static_cast<float>(now - then) / CLOCKS_PER_SEC < 0.09) {
+    if (static_cast<float>(now - then) / CLOCKS_PER_SEC < 0.09) {
       return;
     }
     then = now;
   
     sem_wait(&map_semaphore); {
-    for(unsigned i = 0; i < m_height; i++) {
+      for (unsigned i = 0; i < m_height; i++) {
-      for(unsigned j = 0; j < m_width; j++) {
+	for (unsigned j = 0; j < m_width; j++) {
-	if(MAP_POS(i, j) <= MAX_PHERO_INTENSITY) {
+	  if (MAP_POS(i, j, data) <= MAX_PHERO_INTENSITY) {
-	  p_eva = MAP_POS(i, j) * EVAPORATION_RATE;
+	    p_eva = MAP_POS(i, j, data) * EVAPORATION_RATE;
-	  MAP_POS(i, j) -= p_eva;
+	    MAP_POS(i, j, data) -= p_eva;
-	  MAP_POS(i, j) = MAP_POS(i, j) < 0 ? 0 : MAP_POS(i, j);
+	    MAP_POS(i, j, data) = MAP_POS(i, j, data) < 0 ? 0 : MAP_POS(i, j, data);
 	  }
 	}
       }
     } sem_post(&map_semaphore);
-}
+  }
 
-void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius) {
+  void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius) {
     unsigned int index;
     float        dist;
     float        cos_theta;
     float        ang;
     glm::vec2    v, vp;
   
-  if(sensor == NULL)
+    if (sensor == NULL)
       return;
   
     else {
@@ -180,9 +202,14 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
       v  = glm::normalize(v);
     
       sem_wait(&map_semaphore); {
+	if (sensor_updates == 0) {
+	  memset(sensor_data, 0, m_width * m_height * m_bpp * sizeof(unsigned char));
+	}
+	sensor_updates = (sensor_updates + 1) % 4;
+
 	// For every point in the pheromone map.
-      for(unsigned i = 0; i < m_height; i++) {
+	for (unsigned i = 0; i < m_height; i++) {
-	for(unsigned j = 0; j < m_width; j++) {
+	  for (unsigned j = 0; j < m_width; j++) {
 	    // Calculate the vector from the robot's center to the sampling point.
 	    vp        = glm::vec2(j / float(m_width), 1.0f - (i / float(m_height))) - glm::vec2(x, y);
 	    // Distance from the sampling point to  the robot's center.
@@ -192,7 +219,7 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
 	    cos_theta = glm::dot(vp, v);
 
 	    // Check if the point is close enough to the front of the robot.
-	  if(cos_theta > 0.0f && dist <= radius) {
+	    if (cos_theta > 0.0f && dist <= radius) {
 	      cos_theta = cos_theta > 1.0f ? 1.0f : cos_theta;
 	      // Get the angle between the robot's center and the sampling point.
 	      ang  = PlayerCc::rtod(acos(cos_theta));
@@ -200,9 +227,11 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
 	      // Put the sample in the sampling pool.
 	      index = static_cast<unsigned int>(ang / (180 / 5));
 	      index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index;
-	    sensor->samples[index] = MAP_POS(i, j);
+	      sensor->samples[index] = MAP_POS(i, j, data);
 	      sensor->sample_amnt[index] += 1;
 	    
+	      MAP_POS(i, j, sensor_data) = MAP_POS(i, j, data);
+	      
 	    } else
 	      continue;
 	  }
@@ -210,8 +239,9 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
       } sem_post(&map_semaphore);
 
       // Average the samples.
-    for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) {
+      for (unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) {
 	sensor->samples[i] = (sensor->sample_amnt[i] > 0) ? (sensor->samples[i] / sensor->sample_amnt[i]) : 0.0f;
       }
     }
+  }
 }

pheromone.hpp

@@ -31,12 +31,13 @@
 #include <semaphore.h>
 #include <GL/gl.h>
 
-extern const unsigned char MAX_PHERO_INTENSITY;
+namespace ias_ss {
-extern const unsigned char MIN_PHERO_INTENSITY;
+  extern const unsigned char MAX_PHERO_INTENSITY;
+  extern const unsigned char MIN_PHERO_INTENSITY;
 
-const unsigned int NUM_PHERO_SAMPLES = 180;
+  const unsigned int NUM_PHERO_SAMPLES = 180;
 
-typedef struct PHERO_SENSOR {
+  typedef struct PHERO_SENSOR {
     float        samples[NUM_PHERO_SAMPLES];
     unsigned int sample_amnt[NUM_PHERO_SAMPLES];
     float        probs[NUM_PHERO_SAMPLES];
@@ -59,28 +60,33 @@ typedef struct PHERO_SENSOR {
       else
 	return samples[index];
     }
-} phero_sensor_t;
+  } phero_sensor_t;
 
-class PheromoneMap {
+  class PheromoneMap {
-public:
+  public:
     PheromoneMap(const char * file_name);
     ~PheromoneMap();
 
     GLuint s_build_texture();
+    GLuint s_build_sensor_texture();
     void s_deposit_pheromone(float x, float y, float yaw, float radius);
     void s_evaporate();
     void s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius);
   
-private:
+  private:
     unsigned char * data;
+    unsigned char * sensor_data;
     unsigned        m_width;
     unsigned        m_height;
     unsigned char   m_bpp;
     sem_t           map_semaphore;
     GLuint          handle;
+    GLuint          sensor_handle;
+    int             sensor_updates;
     clock_t         then;
 
     void load_map(const char * file_name);
-};
+  };
+}
 
 #endif

robot.cpp

@@ -25,7 +25,8 @@
 
 #include "robot.hpp"
 
-Robot::Robot(std::string hostname, uint32_t port) {
+namespace ias_ss {
+  Robot::Robot(std::string hostname, uint32_t port) {
     _host_name = hostname;
     _port = port;
 
@@ -36,14 +37,15 @@ Robot::Robot(std::string hostname, uint32_t port) {
     _p_proxy->RequestGeom();
     _r_proxy->RequestGeom();
     _r_proxy->RequestConfigure();
-}
+  }
 
-Robot::~Robot() {
+  Robot::~Robot() {
     delete _p_proxy;
     delete _r_proxy;
     delete _p_client;
-}
+  }
 
-void Robot::log(std::string msg) {
+  void Robot::log(std::string msg) {
     std::cout << "ROBOT(" << _host_name << ":" << _port << ") - " << msg << std::endl;
+  }
 }

robot.hpp

@@ -30,17 +30,18 @@
 #include <string>
 #include <libplayerc++/playerc++.h>
 
-/**
+namespace ias_ss {
+  /**
    * Base class for robot types.
    */
-class Robot {
+  class Robot {
-public:
+  public:
     Robot(std::string hostname, uint32_t port);
     virtual ~Robot();
 
     virtual void run() = 0;
   
-protected:
+  protected:
     int                         _port;
     std::string                 _host_name;
     PlayerCc::PlayerClient *    _p_client;
@@ -48,6 +49,7 @@ protected:
     PlayerCc::RangerProxy *     _r_proxy;
 
     void log(std::string msg);
-};
+  };
+}
 
 #endif