Added sensor rendering.
This commit is contained in:
@@ -35,7 +35,7 @@ window
|
||||
|
||||
floorplan
|
||||
(
|
||||
bitmap "maps/empty.png"
|
||||
bitmap "maps/cave.png"
|
||||
size [ 16.000 16.000 1.500 ]
|
||||
)
|
||||
|
||||
|
70
gui.cpp
70
gui.cpp
@@ -28,35 +28,51 @@
|
||||
#include "gui.hpp"
|
||||
#include "ogl.hpp"
|
||||
|
||||
static void redraw_callback(void * arg) {
|
||||
GlGui * window = static_cast<GlGui *>(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<GlGui *>(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);
|
||||
}
|
||||
|
32
gui.hpp
32
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
|
||||
|
324
ias_robot.cpp
324
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<float>(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * PHERO_RADIUS;
|
||||
}
|
||||
static inline float random_num() {
|
||||
return (((static_cast<float>(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<double>::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<double>::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<int, float> U, V;
|
||||
unsigned int i_min, i_max;
|
||||
float min, sample, prob, max, sum_uv = 0.0f, steer;
|
||||
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;
|
||||
#ifndef NDEBUG
|
||||
std::ostringstream oss;
|
||||
std::ostringstream oss;
|
||||
#endif
|
||||
|
||||
while(U.size() < (U_RATIO * NUM_PHERO_SAMPLES)) {
|
||||
min = std::numeric_limits<double>::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<double>::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<int, float>::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<int, float>::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<double>::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<int, float>::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<int, float>::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<double>::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;
|
||||
}
|
||||
}
|
||||
|
@@ -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
|
||||
|
27
ias_ss.cpp
27
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<IASSS_Robot *>(arg);
|
||||
ias_ss::IASSS_Robot * robot = static_cast<ias_ss::IASSS_Robot *>(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<IASSS_Robot *> robots;
|
||||
std::vector<ias_ss::IASSS_Robot *> 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<void *>(robots[i])) != 0) {
|
||||
perror("Could not create robot thread");
|
||||
|
28
ogl.cpp
28
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;
|
||||
|
3
ogl.hpp
3
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);
|
||||
}
|
||||
|
||||
|
348
pheromone.cpp
348
pheromone.cpp
@@ -27,191 +27,221 @@
|
||||
#include <cstdlib>
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
#include <pnglite.h>
|
||||
#include <glm/glm.hpp>
|
||||
#include <libplayerc++/playerc++.h>
|
||||
|
||||
#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 side(glm::vec3 line, glm::vec3 v) {
|
||||
return sign(glm::cross(line, v).z);
|
||||
}
|
||||
|
||||
static inline float random_n(float r) {
|
||||
return (((static_cast<float>(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;
|
||||
|
||||
glGenTextures(1, &handle);
|
||||
}
|
||||
|
||||
PheromoneMap::~PheromoneMap() {
|
||||
delete data;
|
||||
sem_destroy(&map_semaphore);
|
||||
|
||||
glActiveTexture(GL_TEXTURE0);
|
||||
glBindTexture(GL_TEXTURE_2D, handle);
|
||||
glDeleteTextures(1, &handle);
|
||||
}
|
||||
|
||||
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];
|
||||
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;
|
||||
|
||||
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);
|
||||
|
||||
glActiveTexture(GL_TEXTURE0);
|
||||
glBindTexture(GL_TEXTURE_2D, 0);
|
||||
|
||||
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;
|
||||
|
||||
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<float>(now - then) / CLOCKS_PER_SEC < 0.09) {
|
||||
return;
|
||||
static inline int sign(float f) {
|
||||
return (f < 0.0f) ? -1 : ((f > 0.0f) ? 1 : 0);
|
||||
}
|
||||
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);
|
||||
}
|
||||
static inline int side(glm::vec3 line, glm::vec3 v) {
|
||||
return sign(glm::cross(line, v).z);
|
||||
}
|
||||
|
||||
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;
|
||||
static inline float random_n(float r) {
|
||||
return (((static_cast<float>(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * r;
|
||||
}
|
||||
|
||||
if(sensor == NULL)
|
||||
return;
|
||||
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);
|
||||
}
|
||||
|
||||
else {
|
||||
PheromoneMap::~PheromoneMap() {
|
||||
delete data;
|
||||
sem_destroy(&map_semaphore);
|
||||
|
||||
sensor->reset();
|
||||
glActiveTexture(GL_TEXTURE0);
|
||||
glBindTexture(GL_TEXTURE_2D, handle);
|
||||
glDeleteTextures(1, &handle);
|
||||
glDeleteTextures(1, &sensor_handle);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
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;
|
||||
|
||||
// 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 {
|
||||
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);
|
||||
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<unsigned int>(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;
|
||||
clock_t now = clock();
|
||||
if (static_cast<float>(now - then) / CLOCKS_PER_SEC < 0.09) {
|
||||
return;
|
||||
}
|
||||
then = now;
|
||||
|
||||
} else
|
||||
continue;
|
||||
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<unsigned int>(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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@@ -31,56 +31,62 @@
|
||||
#include <semaphore.h>
|
||||
#include <GL/gl.h>
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
float operator[](unsigned int index) {
|
||||
if(index >= NUM_PHERO_SAMPLES)
|
||||
return -1.0f;
|
||||
else
|
||||
return samples[index];
|
||||
}
|
||||
} phero_sensor_t;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
class PheromoneMap {
|
||||
public:
|
||||
PheromoneMap(const char * file_name);
|
||||
~PheromoneMap();
|
||||
float operator[](unsigned int index) {
|
||||
if(index >= NUM_PHERO_SAMPLES)
|
||||
return -1.0f;
|
||||
else
|
||||
return samples[index];
|
||||
}
|
||||
} phero_sensor_t;
|
||||
|
||||
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);
|
||||
class PheromoneMap {
|
||||
public:
|
||||
PheromoneMap(const char * file_name);
|
||||
~PheromoneMap();
|
||||
|
||||
private:
|
||||
unsigned char * data;
|
||||
unsigned m_width;
|
||||
unsigned m_height;
|
||||
unsigned char m_bpp;
|
||||
sem_t map_semaphore;
|
||||
GLuint handle;
|
||||
clock_t then;
|
||||
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);
|
||||
|
||||
void load_map(const char * file_name);
|
||||
};
|
||||
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);
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
|
40
robot.cpp
40
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;
|
||||
}
|
||||
}
|
||||
|
34
robot.hpp
34
robot.hpp
@@ -30,24 +30,26 @@
|
||||
#include <string>
|
||||
#include <libplayerc++/playerc++.h>
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
Reference in New Issue
Block a user