Pheromone sensing on the way.

Makefile

@@ -27,8 +27,8 @@ TARGET = ias-ss
 OBJECTS = ias_ss.o robot.o ias_robot.o gui.o ogl.o GLSLProgram.o pnglite.o pheromone.o
 DEPENDS = $(OBJECTS:.o=.d)
 CFLAGS = -ansi -pedantic -Wall -I./include
-CXXFLAGS = -ansi -pedantic -Wall `pkg-config --cflags playerc++` -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 -l m -lpthread -lz -lGLEW -lGLU -lGL -lfltk -lfltk_gl
+LDLIBS = `pkg-config --libs playerc++` -lboost_system -lm -lpthread -lz -lGLEW -lGLU -lGL -lfltk -lfltk_gl
 
 all: CXXFLAGS += -O2 -D_NDEBUG
 all: $(TARGET)

ias_robot.cpp

@@ -38,6 +38,7 @@ static const double CRIT_DIST_M      = 1.0;
 static const float  MAP_SIZE         = 16.0f;
 static const int    PHERO_AMOUNT     = 10;
 static const float  PHERO_RADIUS     = 1.0;
+static const float  SENSOR_RADIUS    = 2.0;
 
 static inline float random_num() {
   return (((static_cast<float>(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * PHERO_RADIUS;
@@ -54,14 +55,17 @@ IASSS_Robot::~IASSS_Robot() {
 }
 
 void IASSS_Robot::run() {
+  float x, y;
   int    rv;
   long   then, now, delta, wait;
   struct timeval tv;
   double dist   = std::numeric_limits<double>::infinity();
 
   _p_client->Read();
+
   rv = gettimeofday(&tv, NULL);
   then = tv.tv_usec;
+
   /******************************************************************************
    * WALL AVOIDANCE START                                                       *
    ******************************************************************************/
@@ -78,12 +82,17 @@ void IASSS_Robot::run() {
   /******************************************************************************
    * WALL AVOIDANCE END                                                         *
    ******************************************************************************/
+
+  x = (_p_proxy->GetXPos() + (MAP_SIZE / 2)) / MAP_SIZE;
+  y = (_p_proxy->GetYPos() + (MAP_SIZE / 2)) / MAP_SIZE;
+  _phero_map->s_sample(&_phero_sensor, x, y, _p_proxy->GetYaw(), SENSOR_RADIUS / MAP_SIZE);
+  
+  deposit_pheromone();
+
   rv = gettimeofday(&tv, NULL);
   now = tv.tv_usec;
   delta = now - then;
   
-  deposit_pheromone();
-  
   // Sleep for a bit before finishing this control iteration.
   wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC;
   usleep(wait);
@@ -108,8 +117,8 @@ void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) {
 }
 
 void IASSS_Robot::deposit_pheromone() {
-  float x = _p_proxy->GetXPos();// 
+  float x = _p_proxy->GetXPos();
-  float y = _p_proxy->GetYPos();// + (MAP_SIZE / 2)) / MAP_SIZE;
+  float y = _p_proxy->GetYPos();
   float px, py;
 
   if(_phero_map != NULL) {

ias_robot.hpp

@@ -48,6 +48,7 @@ public:
 
 private:
   PheromoneMap * _phero_map;
+  phero_sensor_t _phero_sensor;
   
   void avoid_wall(float front_speed, float turn_speed);
   void deposit_pheromone();

ogl.cpp

@@ -38,7 +38,6 @@ namespace ogl
   static CGLSLProgram m_program;
   static PheromoneMap * m_phero_map = NULL;
   static GLuint m_textureHandle;
-  static GLuint m_gradientHandle;
 
   // Quad definition
   static glm::vec4 vec_points[6];

pheromone.cpp

@@ -1,6 +1,33 @@
+/*************************************************************************************
+ * Copyright (c) 2016, Miguel Angel Astor Romero                                     *
+ * All rights reserved.                                                              *
+ *                                                                                   *
+ * Redistribution and use in source and binary forms, with or without                *
+ * modification, are permitted provided that the following conditions are met:       *
+ *                                                                                   *
+ * 1. Redistributions of source code must retain the above copyright notice, this    *
+ *    list of conditions and the following disclaimer.                               *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,      *
+ *    this list of conditions and the following disclaimer in the documentation      *
+ *    and/or other materials provided with the distribution.                         *
+ *                                                                                   *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND   *
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED     *
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE            *
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR   *
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES    *
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      *
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND       *
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT        *
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS     *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                      *
+ *************************************************************************************/
+
 #include <iostream>
 #include <cstdlib>
+#include <cmath>
 #include <pnglite.h>
+#include <glm/glm.hpp>
 
 #include "pheromone.hpp"
 
@@ -92,7 +119,7 @@ void PheromoneMap::s_evaporate() {
   sem_wait(&map_semaphore); {
     for(unsigned i = 1; i < m_height - 1; i++) {
       for(unsigned j = 1; j < m_width - 1; j++) {
-	if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY && MAP_POS(i, j) <= MAX_PHERO_INTENSITY){
+	if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY && MAP_POS(i, j) <= MAX_PHERO_INTENSITY) {
 	  p_eva = MAP_POS(i, j) * EVAPORATION_RATE;
 	  MAP_POS(i, j) -= p_eva;
 	}
@@ -100,3 +127,38 @@ void PheromoneMap::s_evaporate() {
     }
   } sem_post(&map_semaphore);
 }
+
+void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius) {
+  int       _x = m_width * x;
+  int       _y = m_height - (m_height * y);
+  float     _r = m_width * radius;
+  float     dist;
+  float     cos_theta;
+  glm::vec2 v;
+  glm::vec2 vp;
+
+  if(sensor == NULL)
+    return;
+  else {
+    v  = glm::vec2(_r * cos(yaw), - _r * sin(yaw)) - glm::vec2(0.0, 0.0);
+    v  = glm::normalize(v);
+
+    sem_wait(&map_semaphore); {
+      for(unsigned i = 1; i < m_height - 1; i++) {
+	for(unsigned j = 1; j < m_width - 1; j++) {
+	  vp        = glm::vec2(i, j) - glm::vec2(_y, _x);
+	  dist      = glm::length(vp);
+	  vp        = glm::normalize(vp);
+	  cos_theta = glm::dot(vp, v);
+
+	  if(cos_theta > 0.0f && dist <= _r) {
+	    if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY && MAP_POS(i, j) <= MAX_PHERO_INTENSITY) {
+	      MAP_POS(i, j) = MAX_PHERO_INTENSITY;
+	    }
+	  } else
+	    continue;
+	}
+      }
+    } sem_post(&map_semaphore);
+  }
+}

pheromone.hpp

@@ -1,6 +1,32 @@
+/*************************************************************************************
+ * Copyright (c) 2016, Miguel Angel Astor Romero                                     *
+ * All rights reserved.                                                              *
+ *                                                                                   *
+ * Redistribution and use in source and binary forms, with or without                *
+ * modification, are permitted provided that the following conditions are met:       *
+ *                                                                                   *
+ * 1. Redistributions of source code must retain the above copyright notice, this    *
+ *    list of conditions and the following disclaimer.                               *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,      *
+ *    this list of conditions and the following disclaimer in the documentation      *
+ *    and/or other materials provided with the distribution.                         *
+ *                                                                                   *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND   *
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED     *
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE            *
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR   *
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES    *
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      *
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND       *
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT        *
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS     *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                      *
+ *************************************************************************************/
+
 #ifndef PHEROMONE_HPP
 #define PHEROMONE_HPP
 
+#include <cstring>
 #include <ctime>
 #include <semaphore.h>
 #include <GL/gl.h>
@@ -8,6 +34,23 @@
 extern const unsigned char MAX_PHERO_INTENSITY;
 extern const unsigned char MIN_PHERO_INTENSITY;
 
+const unsigned int NUM_PHERO_SAMPLES = 180;
+
+typedef struct PHERO_SENSOR {
+  float samples[NUM_PHERO_SAMPLES];
+
+  PHERO_SENSOR() {
+    memset(this->samples, 0.0f, NUM_PHERO_SAMPLES);
+  }
+  
+  float operator[](unsigned int index) {
+    if(index >= NUM_PHERO_SAMPLES)
+      return -1.0f;
+    else
+      return this->samples[index];
+  }
+} phero_sensor_t;
+
 class PheromoneMap {
 public:
   PheromoneMap(const char * file_name);
@@ -16,6 +59,7 @@ public:
   GLuint s_build_texture();
   bool s_deposit_pheromone(float x, float y);
   void s_evaporate();
+  void s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius);
   
 private:
   unsigned char * data;