From 437842a13138dbbd21bc14aa644aa8808758745c Mon Sep 17 00:00:00 2001 From: Miguel Angel Astor Romero Date: Mon, 27 Jun 2016 03:25:45 -0400 Subject: [PATCH] Pheromone sensing on the way. --- Makefile | 4 +-- ias_robot.cpp | 17 ++++++++++--- ias_robot.hpp | 1 + ias_ss.cpp | 6 ++--- ogl.cpp | 1 - pheromone.cpp | 68 ++++++++++++++++++++++++++++++++++++++++++++++++--- pheromone.hpp | 44 +++++++++++++++++++++++++++++++++ 7 files changed, 128 insertions(+), 13 deletions(-) diff --git a/Makefile b/Makefile index 315c006..382fc3b 100644 --- a/Makefile +++ b/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 -LDLIBS = `pkg-config --libs playerc++` -lboost_system -l m -lpthread -lz -lGLEW -lGLU -lGL -lfltk -lfltk_gl +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: $(TARGET) diff --git a/ias_robot.cpp b/ias_robot.cpp index c48af02..a8030b0 100644 --- a/ias_robot.cpp +++ b/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(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::infinity(); _p_client->Read(); + rv = gettimeofday(&tv, NULL); then = tv.tv_usec; + /****************************************************************************** * WALL AVOIDANCE START * ******************************************************************************/ @@ -78,11 +82,16 @@ 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; @@ -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 y = _p_proxy->GetYPos();// + (MAP_SIZE / 2)) / MAP_SIZE; + float x = _p_proxy->GetXPos(); + float y = _p_proxy->GetYPos(); float px, py; if(_phero_map != NULL) { diff --git a/ias_robot.hpp b/ias_robot.hpp index 29fd088..b5f5500 100644 --- a/ias_robot.hpp +++ b/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(); diff --git a/ias_ss.cpp b/ias_ss.cpp index a4d2539..e60b59d 100644 --- a/ias_ss.cpp +++ b/ias_ss.cpp @@ -37,9 +37,9 @@ 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 bool done = false; +static Fl_Window * window = NULL; +static GlGui * glWindow = NULL; static PheromoneMap * phero_map = NULL; extern "C" void handler(int signal) { diff --git a/ogl.cpp b/ogl.cpp index 9bd4a02..a7b6edb 100644 --- a/ogl.cpp +++ b/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]; diff --git a/pheromone.cpp b/pheromone.cpp index ec653ba..7cd109c 100644 --- a/pheromone.cpp +++ b/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 #include +#include #include +#include #include "pheromone.hpp" @@ -67,8 +94,8 @@ GLuint PheromoneMap::s_build_texture() { bool PheromoneMap::s_deposit_pheromone(float x, float y) { bool valid = false; - int _x = m_width * x; - int _y = m_height - (m_height * y); + int _x = m_width * x; + int _y = m_height - (m_height * y); sem_wait(&map_semaphore); { if(MAP_POS(_y, _x) <= MAX_PHERO_INTENSITY) { @@ -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); + } +} diff --git a/pheromone.hpp b/pheromone.hpp index 2e60d6c..4a70b4f 100644 --- a/pheromone.hpp +++ b/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 #include #include #include @@ -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;