From f9aaa9e960d4184f4037e6fba151c7771a347c33 Mon Sep 17 00:00:00 2001 From: Miguel Angel Astor Romero Date: Mon, 6 Mar 2017 15:24:57 -0400 Subject: [PATCH] Added sensor rendering. --- ant.world | 2 +- gui.cpp | 70 +++++++---- gui.hpp | 32 +++-- ias_robot.cpp | 324 ++++++++++++++++++++++++------------------------ ias_robot.hpp | 48 ++++---- ias_ss.cpp | 27 ++-- ogl.cpp | 28 ++++- ogl.hpp | 3 +- pheromone.cpp | 334 +++++++++++++++++++++++++++----------------------- pheromone.hpp | 90 +++++++------- robot.cpp | 40 +++--- robot.hpp | 34 ++--- 12 files changed, 566 insertions(+), 466 deletions(-) diff --git a/ant.world b/ant.world index e517218..12e00db 100644 --- a/ant.world +++ b/ant.world @@ -35,7 +35,7 @@ window floorplan ( - bitmap "maps/empty.png" + bitmap "maps/cave.png" size [ 16.000 16.000 1.500 ] ) diff --git a/gui.cpp b/gui.cpp index 6e9f435..82412c9 100644 --- a/gui.cpp +++ b/gui.cpp @@ -28,35 +28,51 @@ #include "gui.hpp" #include "ogl.hpp" -static void redraw_callback(void * arg) { - GlGui * window = static_cast(arg); - window->redraw(); -} +namespace gui { -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) { - mode(FL_RGB | FL_DOUBLE); - - this->parent = parent; - this->phero_map = phero_map; - title += l; - initialized = false; - - Fl::add_idle(redraw_callback, this); -} - -void GlGui::draw() { - if(!valid()) { - if(!initialized) { - ogl::initialize(phero_map); - initialized = true; - } - ogl::reshape(w(), h()); + static void redraw_callback(void * arg) { + GlGui * window = static_cast(arg); + window->redraw(); } - phero_map->s_evaporate(); - ogl::display(); -} + 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; + this->phero_map = phero_map; + title += l; + initialized = false; + + Fl::add_idle(redraw_callback, this); + } + + void GlGui::draw() { + if(!valid()) { + if(!initialized) { + ogl::initialize(phero_map); + initialized = true; + } + ogl::reshape(w(), h()); + } + + phero_map->s_evaporate(); + ogl::display(); + } + + 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(); + } -int GlGui::handle(int event) { - return Fl_Gl_Window::handle(event); } diff --git a/gui.hpp b/gui.hpp index 10d6a7d..2355b60 100644 --- a/gui.hpp +++ b/gui.hpp @@ -34,19 +34,27 @@ #include "pheromone.hpp" -class GlGui : public Fl_Gl_Window { -public: - GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, PheromoneMap * phero_map); +namespace gui { + class GlGui : public Fl_Gl_Window { + public: + GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l, ias_ss::PheromoneMap * phero_map); -protected: - virtual void draw(); - virtual int handle(int); + protected: + virtual void draw(); + virtual int handle(int); + Fl_Window * parent; + std::string title; + bool initialized; + ias_ss::PheromoneMap * phero_map; + }; -private: - Fl_Window * parent; - std::string title; - bool initialized; - 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 diff --git a/ias_robot.cpp b/ias_robot.cpp index 3363507..f4df147 100644 --- a/ias_robot.cpp +++ b/ias_robot.cpp @@ -36,203 +36,205 @@ //#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.0; -static const double CRIT_DIST_M = 0.7; -static const float MAP_SIZE = 16.0f; -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; +namespace ias_ss { + 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.0; + static const double CRIT_DIST_M = 0.7; + static const float MAP_SIZE = 16.0f; + 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; -} + static inline float random_num() { + return (((static_cast(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) { - srand(port + time(NULL)); - _phero_map = phero_map; - log("Creating IAS-SS robot"); -} + 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() { - log("Destroying IAS-SS robot"); -} + IASSS_Robot::~IASSS_Robot() { + log("Destroying IAS-SS robot"); + } -void IASSS_Robot::run() { - float x, y, steer = 0.0f; - int rv; - long then, now, delta, wait; - struct timeval tv; - double dist = std::numeric_limits::infinity(); + void IASSS_Robot::run() { + float x, y, steer = 0.0f; + int rv; + long then, now, delta, wait; + struct timeval tv; + double dist = std::numeric_limits::infinity(); - _p_client->Read(); + _p_client->Read(); - rv = gettimeofday(&tv, NULL); - then = tv.tv_usec; + rv = gettimeofday(&tv, NULL); + then = tv.tv_usec; - 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); + 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(); + steer += SMOOTHING_COEFF * brss(); - deposit_pheromone(x, y); + deposit_pheromone(x, y); - /****************************************************************************** - * WALL AVOIDANCE START * - ******************************************************************************/ - // Check if there is something in front of the robot. - for(int i = 89; i < 134; i++) - dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist; + /****************************************************************************** + * WALL AVOIDANCE START * + ******************************************************************************/ + // Check if there is something in front of the robot. + 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) { - avoid_wall(METERS_PER_SEC, TURN_DEG_PER_SEC); - } else if(dist <= CRIT_DIST_M) { - avoid_wall(0.0f, TURN_DEG_PER_SEC); - } else - _p_proxy->SetSpeed(METERS_PER_SEC, steer); - /****************************************************************************** - * WALL AVOIDANCE END * - ******************************************************************************/ + if(dist < MIN_DIST_M && dist > CRIT_DIST_M) { + avoid_wall(METERS_PER_SEC, TURN_DEG_PER_SEC); + } else if(dist <= CRIT_DIST_M) { + avoid_wall(0.0f, TURN_DEG_PER_SEC); + } else + _p_proxy->SetSpeed(METERS_PER_SEC, steer); + /****************************************************************************** + * WALL AVOIDANCE END * + ******************************************************************************/ - rv = gettimeofday(&tv, NULL); - now = tv.tv_usec; - delta = now - then; + rv = gettimeofday(&tv, NULL); + now = tv.tv_usec; + delta = now - then; - // Sleep for a bit before finishing this control iteration. - wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC; - usleep(wait); -} + // 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) { - double dist_l = 0.0; - double dist_r = 0.0; + void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) { + double dist_l = 0.0; + double dist_r = 0.0; - for(unsigned int i = 0; i < 96; i++) - dist_r += _r_proxy->GetRange(i); - dist_r /= 96; + for(unsigned int i = 0; i < 96; i++) + dist_r += _r_proxy->GetRange(i); + dist_r /= 96; - for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++) - dist_l += _r_proxy->GetRange(i); - dist_l /= (_r_proxy->GetRangeCount() - 126); + for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++) + dist_l += _r_proxy->GetRange(i); + dist_l /= (_r_proxy->GetRangeCount() - 126); - if(dist_r >= dist_l) - _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(-turn_speed)); - else - _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(turn_speed)); -} + if(dist_r >= dist_l) + _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) { - for(int i = 0; i < PHERO_AMOUNT; i++) - _phero_map->s_deposit_pheromone(x, y, _p_proxy->GetYaw(), PHERO_RADIUS / MAP_SIZE); -} + 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() { - std::map U, V; - unsigned int i_min, i_max; - float min, sample, prob, max, sum_uv = 0.0f, steer; + float IASSS_Robot::brss() { + std::map U, V; + unsigned int i_min, i_max; + float min, sample, prob, max, sum_uv = 0.0f, steer; #ifndef NDEBUG - std::ostringstream oss; + std::ostringstream oss; #endif - 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; + 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; + } + } + } + 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(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(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(U.find(i) == U.end() && V.find(i) == V.end()) { - prob = rand() % 100; - if(prob < 15) - V[i] = _phero_sensor[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(U.find(i) == U.end() && V.find(i) == V.end()) { - prob = rand() % 100; - if(prob < 15) - V[i] = _phero_sensor[i]; + if(_phero_sensor.probs[i] > max) { + max = _phero_sensor.probs[i]; + i_max = 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; + steer = (NUM_PHERO_SAMPLES / 2.0f) - i_max; #ifndef NDEBUG - 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; + 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()); + log(oss.str()); #endif - return steer; + return steer; + } } diff --git a/ias_robot.hpp b/ias_robot.hpp index f5b5d7d..81b3a7b 100644 --- a/ias_robot.hpp +++ b/ias_robot.hpp @@ -29,30 +29,32 @@ #include "robot.hpp" #include "pheromone.hpp" -/** - * Concrete robot that implements the IAS-SS architecture as defined in: - * - * 1) R. Calvo et al. "Inverse ACO for Exploration and Surveillance in - * Unknown Environments", The Third International Conference on Advanced - * Cognitive Technologies and Applications, Rome, Italy 2011. - * - * 2) R. Calvo et al. "A Distributed, Bio-Inspired Coordination Strategy - * for Multiple Agent Systems Applied to Surveillance Tasks in Unknown - * Environments", Proc. of the IEEE IJCNN, San Jose, USA, 2011. - */ -class IASSS_Robot : Robot { -public: - IASSS_Robot(std::string hostname, uint32_t port, PheromoneMap * phero_map); - virtual ~IASSS_Robot(); - virtual void run(); +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 + * Unknown Environments", The Third International Conference on Advanced + * Cognitive Technologies and Applications, Rome, Italy 2011. + * + * 2) R. Calvo et al. "A Distributed, Bio-Inspired Coordination Strategy + * for Multiple Agent Systems Applied to Surveillance Tasks in Unknown + * Environments", Proc. of the IEEE IJCNN, San Jose, USA, 2011. + */ + class IASSS_Robot : Robot { + public: + IASSS_Robot(std::string hostname, uint32_t port, PheromoneMap * phero_map); + virtual ~IASSS_Robot(); + virtual void run(); -private: - PheromoneMap * _phero_map; - phero_sensor_t _phero_sensor; + 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(); -}; + void avoid_wall(float front_speed, float turn_speed); + void deposit_pheromone(float x, float y); + float brss(); + }; +} #endif diff --git a/ias_ss.cpp b/ias_ss.cpp index bd134b1..6b59c23 100644 --- a/ias_ss.cpp +++ b/ias_ss.cpp @@ -32,22 +32,25 @@ #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; const uint32_t NUM_ROBOTS = 4; -static bool done = false; -static Fl_Window * window = NULL; -static GlGui * glWindow = NULL; -static PheromoneMap * phero_map = NULL; +static bool done = false; +static Fl_Window * window = NULL; +static Fl_Window * sWindow = 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(arg); + ias_ss::IASSS_Robot * robot = static_cast(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 robots; + std::vector 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(robots[i])) != 0) { perror("Could not create robot thread"); diff --git a/ogl.cpp b/ogl.cpp index 0eafb41..23e85bd 100644 --- a/ogl.cpp +++ b/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; diff --git a/ogl.hpp b/ogl.hpp index 9d2356f..b156521 100644 --- a/ogl.hpp +++ b/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); } diff --git a/pheromone.cpp b/pheromone.cpp index 893cad3..3d0d39b 100644 --- a/pheromone.cpp +++ b/pheromone.cpp @@ -27,191 +27,221 @@ #include #include #include +#include + #include #include #include #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; -static const float EVAPORATION_RATE = 0.05f; -const unsigned char MAX_PHERO_INTENSITY = 250; -const unsigned char MIN_PHERO_INTENSITY = 0; +namespace ias_ss { + static const unsigned int MAX_ITERS = 1000; + static const float EVAPORATION_RATE = 0.05f; + 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); -} + 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) { - return sign(glm::cross(line, v).z); -} + 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; -} + 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); - then = 0; + 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); + } - glGenTextures(1, &handle); -} + PheromoneMap::~PheromoneMap() { + delete data; + sem_destroy(&map_semaphore); -PheromoneMap::~PheromoneMap() { - delete data; - sem_destroy(&map_semaphore); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, handle); + glDeleteTextures(1, &handle); + glDeleteTextures(1, &sensor_handle); + } - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, handle); - glDeleteTextures(1, &handle); -} + void PheromoneMap::load_map(const char * file_name) { + png_t tex; -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); - 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); - - std::cout << "Loaded map \"" << file_name << "\" :: " << tex.width << "x" << tex.height << "x" << (int)tex.bpp << std::endl; - m_width = tex.width; - m_height = tex.height; - m_bpp = tex.bpp; + std::cout << "Loaded map \"" << file_name << "\" :: " << tex.width << "x" << tex.height << "x" << (int)tex.bpp << std::endl; + m_width = tex.width; + m_height = tex.height; + m_bpp = tex.bpp; - png_close_file(&tex); -} + png_close_file(&tex); + } -GLuint PheromoneMap::s_build_texture() { - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, handle); - sem_wait(&map_semaphore); { - glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, m_width, m_height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, 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); + GLuint PheromoneMap::s_build_texture() { + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, handle); + sem_wait(&map_semaphore); { + glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, m_width, m_height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, 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); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, 0); - return handle; -} + return handle; + } -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; + GLuint PheromoneMap::s_build_sensor_texture() { + 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); - v = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0); - v = glm::normalize(v); + 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; + + v = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0); + v = glm::normalize(v); - 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.09) { - return; - } - then = now; - - sem_wait(&map_semaphore); { - 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); - } - } - } - } sem_post(&map_semaphore); -} - -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) - 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++) { - // 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. + 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); - vp = glm::normalize(vp); - // Cosine of the angle between the robot's center and the sampling point. - cos_theta = glm::dot(vp, v); + } 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, data) <= MAX_PHERO_INTENSITY) { + MAP_POS(_y, _x, data) += rand() % MAX_PHERO_INTENSITY; + 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) + break; + } while(!valid); + } - // 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)); + void PheromoneMap::s_evaporate() { + unsigned char p_eva; - // Put the sample in the sampling pool. - index = static_cast(ang / (180 / 5)); - index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index; - sensor->samples[index] = MAP_POS(i, j); - sensor->sample_amnt[index] += 1; - - } else - continue; + clock_t now = clock(); + if (static_cast(now - then) / CLOCKS_PER_SEC < 0.09) { + return; + } + then = now; + + sem_wait(&map_semaphore); { + for (unsigned i = 0; i < m_height; i++) { + for (unsigned j = 0; j < m_width; j++) { + if (MAP_POS(i, j, data) <= MAX_PHERO_INTENSITY) { + p_eva = MAP_POS(i, j, data) * EVAPORATION_RATE; + MAP_POS(i, j, data) -= p_eva; + MAP_POS(i, j, data) = MAP_POS(i, j, data) < 0 ? 0 : MAP_POS(i, j, data); + } } } } sem_post(&map_semaphore); + } - // 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; + 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) + 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); { + 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 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. + 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)); + + // Put the sample in the sampling pool. + index = static_cast(ang / (180 / 5)); + index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index; + 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; + } + } + } sem_post(&map_semaphore); + + // 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; + } } } } diff --git a/pheromone.hpp b/pheromone.hpp index 78e528c..019a0f7 100644 --- a/pheromone.hpp +++ b/pheromone.hpp @@ -31,56 +31,62 @@ #include #include -extern const unsigned char MAX_PHERO_INTENSITY; -extern const unsigned char MIN_PHERO_INTENSITY; +namespace ias_ss { + 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 { - float samples[NUM_PHERO_SAMPLES]; - unsigned int sample_amnt[NUM_PHERO_SAMPLES]; - float probs[NUM_PHERO_SAMPLES]; + typedef struct PHERO_SENSOR { + float samples[NUM_PHERO_SAMPLES]; + unsigned int sample_amnt[NUM_PHERO_SAMPLES]; + float probs[NUM_PHERO_SAMPLES]; - PHERO_SENSOR() { - reset(); - } - - void reset() { - memset(sample_amnt, 0, sizeof(unsigned int) * NUM_PHERO_SAMPLES); - for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) { - samples[i] = 0.0f; - probs[i] = 0.0f; + PHERO_SENSOR() { + reset(); + } + + void reset() { + memset(sample_amnt, 0, sizeof(unsigned int) * NUM_PHERO_SAMPLES); + for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) { + samples[i] = 0.0f; + probs[i] = 0.0f; + } } - } - float operator[](unsigned int index) { - if(index >= NUM_PHERO_SAMPLES) - return -1.0f; - else - return samples[index]; - } -} phero_sensor_t; + float operator[](unsigned int index) { + if(index >= NUM_PHERO_SAMPLES) + return -1.0f; + else + return samples[index]; + } + } phero_sensor_t; -class PheromoneMap { -public: - PheromoneMap(const char * file_name); - ~PheromoneMap(); + class PheromoneMap { + public: + PheromoneMap(const char * file_name); + ~PheromoneMap(); - GLuint s_build_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); + 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: - unsigned char * data; - unsigned m_width; - unsigned m_height; - unsigned char m_bpp; - sem_t map_semaphore; - GLuint handle; - clock_t then; + 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); -}; + void load_map(const char * file_name); + }; +} #endif diff --git a/robot.cpp b/robot.cpp index e259578..69ea6b8 100644 --- a/robot.cpp +++ b/robot.cpp @@ -25,25 +25,27 @@ #include "robot.hpp" -Robot::Robot(std::string hostname, uint32_t port) { - _host_name = hostname; - _port = port; +namespace ias_ss { + Robot::Robot(std::string hostname, uint32_t port) { + _host_name = hostname; + _port = port; - _p_client = new PlayerCc::PlayerClient(hostname, port); - _p_proxy = new PlayerCc::Position2dProxy(_p_client, 0); - _r_proxy = new PlayerCc::RangerProxy(_p_client, 0); + _p_client = new PlayerCc::PlayerClient(hostname, port); + _p_proxy = new PlayerCc::Position2dProxy(_p_client, 0); + _r_proxy = new PlayerCc::RangerProxy(_p_client, 0); - _p_proxy->RequestGeom(); - _r_proxy->RequestGeom(); - _r_proxy->RequestConfigure(); -} - -Robot::~Robot() { - delete _p_proxy; - delete _r_proxy; - delete _p_client; -} - -void Robot::log(std::string msg) { - std::cout << "ROBOT(" << _host_name << ":" << _port << ") - " << msg << std::endl; + _p_proxy->RequestGeom(); + _r_proxy->RequestGeom(); + _r_proxy->RequestConfigure(); + } + + Robot::~Robot() { + delete _p_proxy; + delete _r_proxy; + delete _p_client; + } + + void Robot::log(std::string msg) { + std::cout << "ROBOT(" << _host_name << ":" << _port << ") - " << msg << std::endl; + } } diff --git a/robot.hpp b/robot.hpp index 7462510..6388c05 100644 --- a/robot.hpp +++ b/robot.hpp @@ -30,24 +30,26 @@ #include #include -/** - * Base class for robot types. - */ -class Robot { -public: - Robot(std::string hostname, uint32_t port); - virtual ~Robot(); +namespace ias_ss { + /** + * Base class for robot types. + */ + class Robot { + public: + Robot(std::string hostname, uint32_t port); + virtual ~Robot(); - virtual void run() = 0; + virtual void run() = 0; -protected: - int _port; - std::string _host_name; - PlayerCc::PlayerClient * _p_client; - PlayerCc::Position2dProxy * _p_proxy; - PlayerCc::RangerProxy * _r_proxy; + protected: + int _port; + std::string _host_name; + PlayerCc::PlayerClient * _p_client; + PlayerCc::Position2dProxy * _p_proxy; + PlayerCc::RangerProxy * _r_proxy; - void log(std::string msg); -}; + void log(std::string msg); + }; +} #endif