WallyHackenslacker/IAS-SS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Pheromone sensing on the way.

Browse Source
This commit is contained in:
Wally Hackenslacker
2016-06-27 03:25:45 -04:00
parent fef86e140a
commit 437842a131
7 changed files with 128 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@@ -27,7 +27,7 @@ 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 -lm -lpthread -lz -lGLEW -lGLU -lGL -lfltk -lfltk_gl
all: CXXFLAGS += -O2 -D_NDEBUG

17
ias_robot.cpp
View File

@@ -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 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) {

1
ias_robot.hpp
View File

@@ -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();

1
ogl.cpp
View File

@@ -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];

62
pheromone.cpp
View File

@@ -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"
@@ -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);
}
}

44
pheromone.hpp
View File

@@ -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;