Testing a different sensor model.

ias_robot.cpp

@@ -28,10 +28,11 @@
 #include <cstdlib>
 #include <cmath>
 #include <sys/time.h>
+#include <sstream>
 
 #include "ias_robot.hpp"
 
-#define PROB_MODEL_1
+//#define PROB_MODEL_1
 
 static const float  TURN_DEG_PER_SEC = 90.0f;
 static const float  METERS_PER_SEC   = 0.4f;
@@ -84,7 +85,7 @@ void IASSS_Robot::run() {
    * WALL AVOIDANCE START                                                       *
    ******************************************************************************/
   // Check if there is something in front of the robot.
-  for(int i = 96; i < 126; i++)
+  for(int i = 89; i < 134; i++)
     dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist;
 
   if(dist < MIN_DIST_M && dist > CRIT_DIST_M) {
@@ -133,6 +134,7 @@ 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;
+  std::ostringstream   oss;
 
   while(U.size() < (U_RATIO * NUM_PHERO_SAMPLES)) {
     min = std::numeric_limits<double>::max();
@@ -198,7 +200,7 @@ float IASSS_Robot::brss() {
 #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);
+    _phero_sensor.probs[i] = (1.0f - _phero_sensor[i]) / (sum_uv);
 #endif
   }
 
@@ -219,5 +221,12 @@ float IASSS_Robot::brss() {
 
   steer = (NUM_PHERO_SAMPLES / 2.0f) - i_max;
 
+  oss << "samples: " << std::endl;
+  for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++)
+    oss << "\tSAMPLE[" << i << "]: " << _phero_sensor[i] << " - " << _phero_sensor.sample_amnt[i] << " - " << _phero_sensor.probs[i] << std::endl;
+  oss << "\ti_max: " << i_max << " | Steer: " << steer;
+  
+  log(oss.str());
+
   return steer;
 }

pheromone.cpp

@@ -187,52 +187,51 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
   float        cos_theta;
   float        ang;
   glm::vec2    v, vp;
-  glm::vec3    line, v3d;
 
   if(sensor == NULL)
     return;
+  
   else {
+    
     sensor->reset();
 
+    // Calculate the robot's view vector.
     v  = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0);
     v  = glm::normalize(v);
     
     sem_wait(&map_semaphore); {
+      // For every point in the pheromone map.
       for(unsigned i = 0; i < m_height; i++) {
 	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);
+	  // 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.
 	  dist      = glm::length(vp);
 	  vp        = glm::normalize(vp);
+	  // Cosine of the angle between the robot's center and the sampling point.
 	  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) {
 	    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));
-	    line = glm::vec3(v.x, v.y, 0.0f);
+
-	    v3d  = glm::vec3(vp.x, vp.y, 0.0f);
+	    // Put the sample in the sampling pool.
-	    if(side(line, v3d) > 0) {
+	    index = static_cast<unsigned int>(ang / (180 / 5));
-	      index = static_cast<unsigned int>((NUM_PHERO_SAMPLES / 2.0f) - ang);
+	    index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index;
-	      index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index;
+	    sensor->samples[index] = MAP_POS(i, j);
-	      if(MAP_POS(i, j) > sensor->samples[index]) {
+	    sensor->sample_amnt[index] += 1;
-		sensor->samples[index] = MAP_POS(i, j);
+
-		sensor->sample_amnt[index] += 1;
-	      }
-	    } else {
-	      index = static_cast<unsigned int>((NUM_PHERO_SAMPLES / 2.0f) + ang);
-	      index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index;
-	      if(MAP_POS(i, j) > sensor->samples[index]) {
-		sensor->samples[index] = MAP_POS(i, j);
-		sensor->sample_amnt[index] += 1;
-	      }
-	    }
 	  } else
 	    continue;
 	}
       }
     } sem_post(&map_semaphore);
 
-    /*for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) {
+    // Average the samples.
+    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

@@ -34,7 +34,7 @@
 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 = 5;
 
 typedef struct PHERO_SENSOR {
   float        samples[NUM_PHERO_SAMPLES];