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

Added sensor rendering.

Browse Source
This commit is contained in:
Wally Hackenslacker
2017-03-06 15:24:57 -04:00
parent 39efccb0e4
commit f9aaa9e960
12 changed files with 566 additions and 466 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
ant.world
View File

@@ -35,7 +35,7 @@ window
floorplan
(
bitmap "maps/empty.png"
bitmap "maps/cave.png"
size [ 16.000 16.000 1.500 ]
)

18
gui.cpp
View File

@@ -28,12 +28,14 @@
#include "gui.hpp"
#include "ogl.hpp"
namespace gui {
static void redraw_callback(void * arg) {
GlGui * window = static_cast<GlGui *>(arg);
window->redraw();
}
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) {
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;
@@ -60,3 +62,17 @@ void GlGui::draw() {
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();
}
}

16
gui.hpp
View File

@@ -34,19 +34,27 @@
#include "pheromone.hpp"
namespace gui {
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);
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);
private:
Fl_Window * parent;
std::string title;
bool initialized;
PheromoneMap * phero_map;
ias_ss::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

2
ias_robot.cpp
View File

@@ -36,6 +36,7 @@
//#define PROB_MODEL_1
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;
@@ -236,3 +237,4 @@ float IASSS_Robot::brss() {
return steer;
}
}

2
ias_robot.hpp
View File

@@ -29,6 +29,7 @@
#include "robot.hpp"
#include "pheromone.hpp"
namespace ias_ss {
/**
* Concrete robot that implements the IAS-SS architecture as defined in:
*
@@ -54,5 +55,6 @@ private:
void deposit_pheromone(float x, float y);
float brss();
};
}
#endif

23
ias_ss.cpp
View File

@@ -32,6 +32,7 @@
#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;
@@ -39,15 +40,17 @@ 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 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
View File

@@ -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
View File

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

52
pheromone.cpp
View File

@@ -27,14 +27,17 @@
#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)])
namespace ias_ss {
static const unsigned int MAX_ITERS = 1000;
static const float EVAPORATION_RATE = 0.05f;
const unsigned char MAX_PHERO_INTENSITY = 250;
@@ -56,8 +59,9 @@ 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);
}
PheromoneMap::~PheromoneMap() {
@@ -67,6 +71,7 @@ PheromoneMap::~PheromoneMap() {
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, handle);
glDeleteTextures(1, &handle);
glDeleteTextures(1, &sensor_handle);
}
void PheromoneMap::load_map(const char * file_name) {
@@ -75,6 +80,7 @@ void PheromoneMap::load_map(const char * file_name) {
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;
@@ -100,8 +106,24 @@ GLuint PheromoneMap::s_build_texture() {
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, _x, _y;
unsigned int iters = 0;
int _x, _y;
bool valid = false;
float dist, cos_theta, r_x, r_y;
glm::vec2 v, vp;
@@ -127,9 +149,9 @@ void PheromoneMap::s_deposit_pheromone(float x, float y, float yaw, float radius
_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);
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);
@@ -151,10 +173,10 @@ void PheromoneMap::s_evaporate() {
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);
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);
}
}
}
@@ -180,6 +202,11 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
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++) {
@@ -200,9 +227,11 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
// 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->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;
}
@@ -215,3 +244,4 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
}
}
}
}

6
pheromone.hpp
View File

@@ -31,6 +31,7 @@
#include <semaphore.h>
#include <GL/gl.h>
namespace ias_ss {
extern const unsigned char MAX_PHERO_INTENSITY;
extern const unsigned char MIN_PHERO_INTENSITY;
@@ -67,20 +68,25 @@ public:
~PheromoneMap();
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 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

2
robot.cpp
View File

@@ -25,6 +25,7 @@
#include "robot.hpp"
namespace ias_ss {
Robot::Robot(std::string hostname, uint32_t port) {
_host_name = hostname;
_port = port;
@@ -47,3 +48,4 @@ Robot::~Robot() {
void Robot::log(std::string msg) {
std::cout << "ROBOT(" << _host_name << ":" << _port << ") - " << msg << std::endl;
}
}

2
robot.hpp
View File

@@ -30,6 +30,7 @@
#include <string>
#include <libplayerc++/playerc++.h>
namespace ias_ss {
/**
* Base class for robot types.
*/
@@ -49,5 +50,6 @@ protected:
void log(std::string msg);
};
}
#endif