diff --git a/Makefile b/Makefile index 382fc3b..4b8277e 100644 --- a/Makefile +++ b/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 diff --git a/README.org b/README.org index 972761b..95038b7 100644 --- a/README.org +++ b/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. + diff --git a/ias_robot.cpp b/ias_robot.cpp index aa398ec..e11e50c 100644 --- a/ias_robot.cpp +++ b/ias_robot.cpp @@ -23,6 +23,7 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * *************************************************************************************/ +#include #include #include #include @@ -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 CRIT_DIST_M = 1.0; +static const double MIN_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 float PHERO_RADIUS = 1.0; -static const float SENSOR_RADIUS = 2.0; +static const int PHERO_AMOUNT = 30; +static const float PHERO_RADIUS = 1.4f; +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(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * PHERO_RADIUS; @@ -55,11 +61,11 @@ 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; - double dist = std::numeric_limits::infinity(); + double dist = std::numeric_limits::infinity(); _p_client->Read(); @@ -69,7 +75,11 @@ void IASSS_Robot::run() { 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); + + steer += SMOOTHING_COEFF * brss(); + deposit_pheromone(x, y); + /****************************************************************************** * WALL AVOIDANCE START * ******************************************************************************/ @@ -82,12 +92,10 @@ 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; @@ -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() { - float x = _p_proxy->GetXPos(); - float y = _p_proxy->GetYPos(); - float px, py; +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); +} - if(_phero_map != NULL) { - for(int i = 0; i < PHERO_AMOUNT; i++) { - px = random_num() + x; - py = random_num() + y; - if(fabs(px) < (MAP_SIZE / 2) && fabs(py) < (MAP_SIZE / 2)) { - px = (px + (MAP_SIZE / 2)) / MAP_SIZE; - py = (py + (MAP_SIZE / 2)) / MAP_SIZE; - if(!_phero_map->s_deposit_pheromone(px, py)) { - i--; - break; +float IASSS_Robot::brss() { + std::map U, V; + unsigned int i_min, i_max; + float min, sample, prob, max, sum_uv = 0.0f, steer; + + while(U.size() < (U_RATIO * NUM_PHERO_SAMPLES)) { + min = std::numeric_limits::max(); + i_min = 0; + for(unsigned int i = NUM_PHERO_SAMPLES / 2; i < NUM_PHERO_SAMPLES; i++) { + sample = _phero_sensor[i]; + if(U.find(i) == U.end()) { + if(sample < min) { + min = sample; + i_min = i; } - } else { - i--; - break; + } + } + 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::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::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::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; } diff --git a/ias_robot.hpp b/ias_robot.hpp index b5f5500..f5b5d7d 100644 --- a/ias_robot.hpp +++ b/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 diff --git a/maps/cave_mask.png b/maps/cave_mask.png index d373865..296504d 100644 Binary files a/maps/cave_mask.png and b/maps/cave_mask.png differ diff --git a/maps/cave_mask_.png b/maps/cave_mask_.png new file mode 100644 index 0000000..d373865 Binary files /dev/null and b/maps/cave_mask_.png differ diff --git a/maps/cave_mask_copy.png b/maps/cave_mask_copy.png deleted file mode 100644 index 7e06d6b..0000000 Binary files a/maps/cave_mask_copy.png and /dev/null differ diff --git a/pheromone.cpp b/pheromone.cpp index b18972e..1ccd62a 100644 --- a/pheromone.cpp +++ b/pheromone.cpp @@ -29,14 +29,16 @@ #include #include #include +#include #include "pheromone.hpp" #define MAP_POS(X, Y) (data[((X) * m_height) + (Y)]) -static const float EVAPORATION_RATE = 0.05f; -const unsigned char MAX_PHERO_INTENSITY = 255; -const unsigned char MIN_PHERO_INTENSITY = 1; +static const unsigned int MAX_ITERS = 1000; +static const float EVAPORATION_RATE = 0.1f; +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(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]; - png_get_data(&tex, data); + _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; 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() { @@ -87,11 +102,15 @@ GLuint PheromoneMap::s_build_texture() { glBindTexture(GL_TEXTURE_2D, handle); glDeleteTextures(1, &handle); } - + + for(unsigned int i = 0; i < (m_width * m_height * m_bpp); i++) { + tex_data[i] = static_cast(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) { - bool valid = false; - unsigned int _x = m_width * x; - unsigned int _y = m_height - (m_height * y); +void PheromoneMap::s_deposit_pheromone(float x, float y, float yaw, float radius) { + unsigned int iters = 0, _x, _y; + bool valid = false; + float dist, cos_theta, r_x, r_y; + glm::vec2 v, vp; - if((y > 1.0 || y < 0.0) || (x > 1.0 || x < 0.0)) - return false; - if((_y >= m_height || _y < 0) || (_x >= m_width || _x < 0)) - return false; + v = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0); + v = glm::normalize(v); - sem_wait(&map_semaphore); { - if(MAP_POS(_y, _x) >= MIN_PHERO_INTENSITY) { - MAP_POS(_y, _x) = MAX_PHERO_INTENSITY; - valid = true; - } - } sem_post(&map_semaphore); - - return valid; + 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) <= 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++; + if(iters > MAX_ITERS) + break; + } while(!valid); } void PheromoneMap::s_evaporate() { unsigned char p_eva; clock_t now = clock(); - if(static_cast(now - then) / CLOCKS_PER_SEC < 0.05) { + if(static_cast(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 j = 1; j < m_width - 1; j++) { - if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY) { + for(unsigned i = 0; i < m_height; i++) { + for(unsigned j = 0; j < m_width; j++) { + 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; - float _y = y; - float dist; - float cos_theta; - float ang; - glm::vec2 v; - glm::vec2 vp; - glm::vec3 line; - glm::vec3 v3d; + unsigned int index; + float dist; + float cos_theta; + float ang; + glm::vec2 v, vp; + glm::vec3 line, 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 j = 1; j < m_width - 1; j++) { - vp = glm::vec2(j/float(m_width), 1.0f - (i/float(m_height))) - glm::vec2(_x, _y); + 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); 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(90.0f - ang) <= 90); - sensor->samples[static_cast(90.0f - ang)] += MAP_POS(i, j); - sensor->sample_amnt[static_cast(90.0f - ang)] += 1; - //MAP_POS(i, j) = MAX_PHERO_INTENSITY; + index = static_cast((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 { - assert(static_cast(90.0f + ang) < 180); - sensor->samples[static_cast(90.0f + ang)] += MAP_POS(i, j); - sensor->sample_amnt[static_cast(90.0f + ang)] += 1; - //MAP_POS(i, j) = MAX_PHERO_INTENSITY; + index = static_cast((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; @@ -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; + } } } diff --git a/pheromone.hpp b/pheromone.hpp index 3c148c8..01eb95b 100644 --- a/pheromone.hpp +++ b/pheromone.hpp @@ -52,17 +52,6 @@ typedef struct PHERO_SENSOR { probs[i] = 0.0f; } } - - 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) @@ -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;