Complete enough.

Makefile

@@ -30,7 +30,7 @@ CFLAGS = -ansi -pedantic -Wall -I./include
 CXXFLAGS = -ansi -pedantic -Wall `pkg-config --cflags playerc++` -I./include -DGLM_FORCE_RADIANS -DBOOST_SIGNALS_NO_DEPRECATION_WARNING
 LDLIBS = `pkg-config --libs playerc++` -lboost_system -lm -lpthread -lz -lGLEW -lGLU -lGL -lfltk -lfltk_gl
 
-all: CXXFLAGS += -O2 -D_NDEBUG
+all: CXXFLAGS += -O2 -DNDEBUG
 all: $(TARGET)
 
 debug: CXXFLAGS += -g

README.org

@@ -9,8 +9,9 @@ following papers:
 
 This is being developed as part of the Robotics and Bio-Inspired Learning graduate course at UCV.
 
-Future versions of this README file will include a technical report on the developed software.
-
 ** Notes
 
-The file ~maps/cave.png~ is part of the standard Stage distribution.
+ * The file ~maps/cave.png~ is part of the standard Stage distribution.
+ * This software uses the /pnglite/ library by Daniel Karling.
+ * The ~GLSLProgram~ code was written by Esmitt Ramirez J.
+

ias_robot.cpp

@@ -23,6 +23,7 @@
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                      *
  *************************************************************************************/
 
+#include <map>
 #include <limits>
 #include <cstdlib>
 #include <cmath>
@@ -30,15 +31,20 @@
 
 #include "ias_robot.hpp"
 
-static const float  TURN_DEG_PER_SEC = 40.0f;
+#define PROB_MODEL_1
+
+static const float  TURN_DEG_PER_SEC = 90.0f;
 static const float  METERS_PER_SEC   = 0.4f;
 static const long   HALF_SECOND_USEC = 500000;
-static const double MIN_DIST_M       = 1.5;
+static const double MIN_DIST_M       = 1.0;
-static const double CRIT_DIST_M      = 1.0;
+static const double CRIT_DIST_M      = 0.7;
 static const float  MAP_SIZE         = 16.0f;
-static const int    PHERO_AMOUNT     = 25;
+static const int    PHERO_AMOUNT     = 30;
-static const float  PHERO_RADIUS     = 1.0;
+static const float  PHERO_RADIUS     = 1.4f;
-static const float  SENSOR_RADIUS    = 2.0;
+static const float  SENSOR_RADIUS    = 2.0f;
+static const float  U_RATIO          = 0.3f;
+static const float  V_RATIO          = 0.1f;
+static const float  SMOOTHING_COEFF  = 0.3f;
 
 static inline float random_num() {
   return (((static_cast<float>(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * PHERO_RADIUS;
@@ -55,7 +61,7 @@ IASSS_Robot::~IASSS_Robot() {
 }
 
 void IASSS_Robot::run() {
-  float x, y;
+  float x, y, steer = 0.0f;
   int    rv;
   long   then, now, delta, wait;
   struct timeval tv;
@@ -70,6 +76,10 @@ void IASSS_Robot::run() {
   y = (_p_proxy->GetYPos() + (MAP_SIZE / 2)) / MAP_SIZE;
   _phero_map->s_sample(&_phero_sensor, x, y, _p_proxy->GetYaw(), SENSOR_RADIUS / MAP_SIZE);
   
+  steer += SMOOTHING_COEFF * brss();
+
+  deposit_pheromone(x, y);
+  
   /******************************************************************************
    * WALL AVOIDANCE START                                                       *
    ******************************************************************************/
@@ -82,13 +92,11 @@ void IASSS_Robot::run() {
   } else if(dist <= CRIT_DIST_M) {
     avoid_wall(0.0f, TURN_DEG_PER_SEC);
   } else
-    _p_proxy->SetSpeed(METERS_PER_SEC, 0.0f);
+    _p_proxy->SetSpeed(METERS_PER_SEC, steer);
   /******************************************************************************
    * WALL AVOIDANCE END                                                         *
    ******************************************************************************/
 
-  deposit_pheromone();
-
   rv = gettimeofday(&tv, NULL);
   now = tv.tv_usec;
   delta = now - then;
@@ -116,26 +124,100 @@ void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) {
     _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(turn_speed));
 }
 
-void IASSS_Robot::deposit_pheromone() {
+void IASSS_Robot::deposit_pheromone(float x, float y) {
-  float x = _p_proxy->GetXPos();
+  for(int i = 0; i < PHERO_AMOUNT; i++)
-  float y = _p_proxy->GetYPos();
+    _phero_map->s_deposit_pheromone(x, y, _p_proxy->GetYaw(), PHERO_RADIUS / MAP_SIZE);
-  float px, py;
+}
-
+
-  if(_phero_map != NULL) {
+float IASSS_Robot::brss() {
-    for(int i = 0; i < PHERO_AMOUNT; i++) {
+  std::map<int, float> U, V;
-      px = random_num() + x;
+  unsigned int         i_min, i_max;
-      py = random_num() + y;
+  float                min, sample, prob, max, sum_uv = 0.0f, steer;
-      if(fabs(px) < (MAP_SIZE / 2) && fabs(py) < (MAP_SIZE / 2)) {
+
-	px = (px + (MAP_SIZE / 2)) / MAP_SIZE;
+  while(U.size() < (U_RATIO * NUM_PHERO_SAMPLES)) {
-	py = (py + (MAP_SIZE / 2)) / MAP_SIZE;
+    min = std::numeric_limits<double>::max();
-	if(!_phero_map->s_deposit_pheromone(px, py)) {
+    i_min = 0;
-	  i--;
+    for(unsigned int i = NUM_PHERO_SAMPLES / 2; i < NUM_PHERO_SAMPLES; i++) {
-	  break;
+      sample = _phero_sensor[i];
-	}
+      if(U.find(i) == U.end()) {
-      } else {
+	if(sample < min) {
-	i--;
+	  min = sample;
-	break;
+	  i_min = i;
-      }
+	}
-    }
+      }
-  }
+    }
+    for(unsigned int i = NUM_PHERO_SAMPLES / 2; i > 0; i--) {
+      sample = _phero_sensor[i];
+      if(U.find(i) == U.end()) {
+	if(sample < min) {
+	  min = sample;
+	  i_min = i;
+	}
+      }
+    }
+    U[i_min] = min;
+  }
+
+  while(V.size() < (V_RATIO * NUM_PHERO_SAMPLES)) {
+    for(unsigned int i = NUM_PHERO_SAMPLES / 2; i < NUM_PHERO_SAMPLES; i++) {
+      if(U.find(i) == U.end() && V.find(i) == V.end()) {
+	prob = rand() % 100;
+	if(prob < 15)
+	  V[i] = _phero_sensor[i];
+      }
+    }
+    for(unsigned int i = NUM_PHERO_SAMPLES / 2; i > 0; i--) {
+      if(U.find(i) == U.end() && V.find(i) == V.end()) {
+	prob = rand() % 100;
+	if(prob < 15)
+	  V[i] = _phero_sensor[i];
+      }
+    }
+  }
+
+  for(std::map<int, float>::iterator it = U.begin(); it != U.end(); ++it) {
+#ifdef PROB_MODEL_1
+    sum_uv += it->second;
+#else
+    sum_uv += 1.0f - it->second;
+#endif
+  }
+
+  for(std::map<int, float>::iterator it = V.begin(); it != V.end(); ++it) {
+#ifdef PROB_MODEL_1
+    sum_uv += it->second;
+#else
+    sum_uv += 1.0f - it->second;
+#endif
+  }
+
+  U.clear();
+  V.clear();
+
+  for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) {
+#ifdef PROB_MODEL_1
+    _phero_sensor.probs[i] = 1.0f / (_phero_sensor[i] / sum_uv);
+#else
+    _phero_sensor.probs[i] = (1.0f - _phero_sensor[i]) / ( / sum_uv);
+#endif
+  }
+
+  max = std::numeric_limits<double>::min();
+  i_max = 0;
+  for(unsigned int i = NUM_PHERO_SAMPLES / 2; i < NUM_PHERO_SAMPLES; i++) {
+    if(_phero_sensor.probs[i] > max) {
+      max = _phero_sensor.probs[i];
+      i_max = i;
+    }
+  }
+  for(unsigned int i = NUM_PHERO_SAMPLES / 2; i > 0; i--) {
+    if(_phero_sensor.probs[i] > max) {
+      max = _phero_sensor.probs[i];
+      i_max = i;
+    }
+  }
+
+  steer = (NUM_PHERO_SAMPLES / 2.0f) - i_max;
+
+  return steer;
 }

ias_robot.hpp

@@ -51,7 +51,8 @@ private:
   phero_sensor_t _phero_sensor;
   
   void avoid_wall(float front_speed, float turn_speed);
-  void deposit_pheromone();
+  void deposit_pheromone(float x, float y);
+  float brss();
 };
 
 #endif

pheromone.cpp

@@ -29,14 +29,16 @@
 #include <cassert>
 #include <pnglite.h>
 #include <glm/glm.hpp>
+#include <libplayerc++/playerc++.h>
 
 #include "pheromone.hpp"
 
 #define MAP_POS(X, Y) (data[((X) * m_height) + (Y)])
 
-static const float         EVAPORATION_RATE    = 0.05f;
+static const unsigned int  MAX_ITERS           = 1000;
-const        unsigned char MAX_PHERO_INTENSITY = 255;
+static const float         EVAPORATION_RATE    = 0.1f;
-const        unsigned char MIN_PHERO_INTENSITY = 1;
+const        unsigned char MAX_PHERO_INTENSITY = 250;
+const        unsigned char MIN_PHERO_INTENSITY = 0;
 
 static inline int sign(float f) {
   return (f < 0.0f) ? -1 : ((f > 0.0f) ? 1 : 0);
@@ -46,6 +48,10 @@ static inline int side(glm::vec3 line, glm::vec3 v) {
   return sign(glm::cross(line, v).z);
 }
 
+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) {
   load_map(file_name);
   sem_init(&map_semaphore, 0, 1);
@@ -55,6 +61,7 @@ PheromoneMap::PheromoneMap(const char * file_name) {
 
 PheromoneMap::~PheromoneMap() {
   delete data;
+  delete tex_data;
   sem_destroy(&map_semaphore);
 
   if(tex_created) {
@@ -65,12 +72,13 @@ PheromoneMap::~PheromoneMap() {
 }
 
 void PheromoneMap::load_map(const char * file_name) {
+  unsigned char * _data;
   png_t tex;
 
   png_init(0, 0);
   png_open_file_read(&tex, file_name);
-  data = new unsigned char[tex.width * tex.height * tex.bpp];
+  _data = new unsigned char[tex.width * tex.height * tex.bpp];
-  png_get_data(&tex, data);
+  png_get_data(&tex, _data);
 
   std::cout << "Loaded map \"" << file_name << "\" :: " << tex.width << "x" << tex.height << "x" << (int)tex.bpp << std::endl;
   m_width  = tex.width;
@@ -78,6 +86,13 @@ void PheromoneMap::load_map(const char * file_name) {
   m_bpp    = tex.bpp;
   
   png_close_file(&tex);
+
+  data = new int[tex.width * tex.height * tex.bpp];
+  tex_data = new unsigned char[tex.width * tex.height * tex.bpp];
+  for(unsigned int i = 0; i < (tex.width * tex.height * tex.bpp); i++) {
+    data[i] = _data[i];
+  }
+  delete _data;
 }
 
 GLuint PheromoneMap::s_build_texture() {
@@ -88,10 +103,14 @@ GLuint PheromoneMap::s_build_texture() {
       glDeleteTextures(1, &handle);
     }
 
+    for(unsigned int i = 0; i < (m_width * m_height * m_bpp); i++) {
+      tex_data[i] = static_cast<unsigned char>(data[i]);
+    }
+
     glGenTextures(1, &handle);
     glActiveTexture(GL_TEXTURE0);
     glBindTexture(GL_TEXTURE_2D, handle);
-    glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, m_width, m_height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, data);
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, m_width, m_height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, tex_data);
     glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
     glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 
@@ -101,41 +120,61 @@ GLuint PheromoneMap::s_build_texture() {
   return handle;
 }
 
-bool PheromoneMap::s_deposit_pheromone(float x, float y) {
+void PheromoneMap::s_deposit_pheromone(float x, float y, float yaw, float radius) {
+  unsigned int iters = 0, _x, _y;
   bool         valid = false;
-  unsigned int  _x    = m_width * x;
+  float        dist, cos_theta, r_x, r_y;
-  unsigned int  _y    = m_height - (m_height * y);
+  glm::vec2    v, vp;
 
-  if((y > 1.0 || y < 0.0) || (x > 1.0 || x < 0.0))
+  v  = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0);
-    return false;
+  v  = glm::normalize(v);
-  if((_y >= m_height || _y < 0) || (_x >= m_width || _x < 0))
-    return false;
 
+  do {
+    do {
+      do {
+	do {
+	  r_x  = random_n(radius) + x;
+	  r_y  = random_n(radius) + y;
+	} while((r_x <= 0.0f && r_y <= 0.0f) || (r_x >= 1.0f && r_y >= 1.0f));
+	vp        = glm::vec2(r_x, r_y) - glm::vec2(x, y);
+	dist      = glm::length(vp);
+      } while(dist > radius);
+      vp        = glm::normalize(vp);
+      cos_theta = glm::dot(vp, v);
+    } while(cos_theta > 0.0f);
+    _x = m_width * r_x;
+    _y = m_height - (m_height * r_y);
+    _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) >= MIN_PHERO_INTENSITY) {
+      if(MAP_POS(_y, _x) <= MAX_PHERO_INTENSITY) {
-      MAP_POS(_y, _x) = MAX_PHERO_INTENSITY;
+	MAP_POS(_y, _x) += rand() % MAX_PHERO_INTENSITY;
+	MAP_POS(_y, _x) = MAP_POS(_y, _x) > MAX_PHERO_INTENSITY ? MAX_PHERO_INTENSITY : MAP_POS(_y, _x);
 	valid = true;
       }
     } sem_post(&map_semaphore);
-
+    iters++;
-  return valid;
+    if(iters > MAX_ITERS)
+      break;
+  } while(!valid);
 }
 
 void PheromoneMap::s_evaporate() {
   unsigned char p_eva;
 
   clock_t now = clock();
-  if(static_cast<float>(now - then) / CLOCKS_PER_SEC < 0.05) {
+  if(static_cast<float>(now - then) / CLOCKS_PER_SEC < 0.09) {
     return;
   }
   then = now;
   
   sem_wait(&map_semaphore); {
-    for(unsigned i = 1; i < m_height - 1; i++) {
+    for(unsigned i = 0; i < m_height; i++) {
-      for(unsigned j = 1; j < m_width - 1; j++) {
+      for(unsigned j = 0; j < m_width; j++) {
-	if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY) {
+	if(MAP_POS(i, j) <= MAX_PHERO_INTENSITY) {
 	  p_eva = MAP_POS(i, j) * EVAPORATION_RATE;
 	  MAP_POS(i, j) -= p_eva;
+	  MAP_POS(i, j) = MAP_POS(i, j) < 0 ? 0 : MAP_POS(i, j);
 	}
       }
     }
@@ -143,15 +182,12 @@ void PheromoneMap::s_evaporate() {
 }
 
 void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius) {
-  float     _x = x;
+  unsigned int index;
-  float     _y = y;
   float        dist;
   float        cos_theta;
   float        ang;
-  glm::vec2 v;
+  glm::vec2    v, vp;
-  glm::vec2 vp;
+  glm::vec3    line, v3d;
-  glm::vec3 line;
-  glm::vec3 v3d;
 
   if(sensor == NULL)
     return;
@@ -162,27 +198,32 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
     v  = glm::normalize(v);
     
     sem_wait(&map_semaphore); {
-      for(unsigned i = 1; i < m_height - 1; i++) {
+      for(unsigned i = 0; i < m_height; i++) {
-	for(unsigned j = 1; j < m_width - 1; j++) {
+	for(unsigned j = 0; j < m_width; j++) {
-	  vp        = glm::vec2(j/float(m_width), 1.0f - (i/float(m_height))) - glm::vec2(_x, _y);
+	  vp        = glm::vec2(j/float(m_width), 1.0f - (i/float(m_height))) - glm::vec2(x, y);
 	  dist      = glm::length(vp);
 	  vp        = glm::normalize(vp);
 	  cos_theta = glm::dot(vp, v);
 
 	  if(cos_theta > 0.0f && dist <= radius) {
-	    ang  = acos(cos_theta);
+	    cos_theta = cos_theta > 1.0f ? 1.0f : cos_theta;
+	    ang  = PlayerCc::rtod(acos(cos_theta));
 	    line = glm::vec3(v.x, v.y, 0.0f);
 	    v3d  = glm::vec3(vp.x, vp.y, 0.0f);
 	    if(side(line, v3d) > 0) {
-	      assert(static_cast<unsigned int>(90.0f - ang) <= 90);
+	      index = static_cast<unsigned int>((NUM_PHERO_SAMPLES / 2.0f) - ang);
-	      sensor->samples[static_cast<unsigned int>(90.0f - ang)] += MAP_POS(i, j);
+	      index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index;
-	      sensor->sample_amnt[static_cast<unsigned int>(90.0f - ang)] += 1;
+	      if(MAP_POS(i, j) > sensor->samples[index]) {
-	      //MAP_POS(i, j) = MAX_PHERO_INTENSITY;
+		sensor->samples[index] += MAP_POS(i, j);
+		sensor->sample_amnt[index] += 1;
+	      }
 	    } else {
-	      assert(static_cast<unsigned int>(90.0f + ang) < 180);
+	      index = static_cast<unsigned int>((NUM_PHERO_SAMPLES / 2.0f) + ang);
-	      sensor->samples[static_cast<unsigned int>(90.0f + ang)] += MAP_POS(i, j);
+	      index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index;
-	      sensor->sample_amnt[static_cast<unsigned int>(90.0f + ang)] += 1;
+	      if(MAP_POS(i, j) > sensor->samples[index]) {
-	      //MAP_POS(i, j) = MAX_PHERO_INTENSITY;
+		sensor->samples[index] += MAP_POS(i, j);
+		sensor->sample_amnt[index] += 1;
+	      }
 	    }
 	  } else
 	    continue;
@@ -190,7 +231,8 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
       }
     } sem_post(&map_semaphore);
 
-    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

@@ -53,17 +53,6 @@ typedef struct PHERO_SENSOR {
     }
   }
   
-  void set_probabilities() {
-    float phero_sum = 0.0f;
-
-    for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++)
-      phero_sum += samples[i];
-    
-    for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) {
-      probs[i] = 1.0f / (samples[i] / phero_sum);
-    }
-  }
-  
   float operator[](unsigned int index) {
     if(index >= NUM_PHERO_SAMPLES)
       return -1.0f;
@@ -78,12 +67,13 @@ public:
   ~PheromoneMap();
 
   GLuint s_build_texture();
-  bool s_deposit_pheromone(float x, float y);
+  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:
-  unsigned char * data;
+  int *           data;
+  unsigned char * tex_data;
   unsigned        m_width;
   unsigned        m_height;
   unsigned char   m_bpp;