Started adding an OpenGL gui for the pheromone map.

Makefile

@@ -23,10 +23,10 @@
 
 CXX = g++
 TARGET = ias-ss
-OBJECTS = ias_ss.o robot.o ias_robot.o
+OBJECTS = ias_ss.o robot.o ias_robot.o gui.o
 DEPENDS = $(OBJECTS:.o=.d)
 CXXFLAGS = -ansi -pedantic -Wall `pkg-config --cflags playerc++`
-LDLIBS = `pkg-config --libs playerc++` -lboost_system -lpthread
+LDLIBS = `pkg-config --libs playerc++` -lboost_system -lpthread -lGLU -lGL -lfltk -lfltk_gl
 
 all: CXXFLAGS += -O2 -D_NDEBUG
 all: $(TARGET)

ant_bot.inc

@@ -51,6 +51,5 @@ define ant_bot position
 
 	drive "diff"
 	localization "gps"
-
 	ant_sonars()
 )

gui.cpp

@@ -0,0 +1,37 @@
+#include <iostream>
+
+#include "gui.hpp"
+
+static void redraw_callback(void * arg) {
+  Fl_Gl_Window * window = static_cast<Fl_Gl_Window *>(arg);
+  window->redraw();
+  Fl::repeat_timeout(0.016, redraw_callback, window);
+}
+
+GlGui::GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l) : Fl_Gl_Window(x, y, w, h, l) {
+  this->parent = parent;
+  title += l;
+  initialized = false;
+  Fl::add_timeout(0.016, redraw_callback, this);
+  resizable(this);
+}
+
+void GlGui::draw() {
+  if(!valid()) {
+    if(!initialized) {
+      //opengl::initialize();
+      
+      std::cout << "OpenGL Version: " << (char*)glGetString(GL_VERSION) << std::endl;
+      std::cout << "OpenGL Vendor: " << (char*)glGetString(GL_VENDOR) << std::endl;
+
+      initialized = true;
+    }
+    //opengl::reshape(w(), h());
+  }
+
+  //opengl::display();
+}
+
+int GlGui::handle(int event) {
+  return Fl_Gl_Window::handle(event);
+}

gui.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#ifndef GUI_HPP
+#define GUI_HPP
+
+#include <FL/Fl.H>
+#include <FL/Fl_Window.H>
+#include <FL/Fl_Gl_Window.H>
+#include <FL/gl.h>
+
+#include <string>
+
+class GlGui : public Fl_Gl_Window {
+public:
+  GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l);
+
+protected:
+  virtual void draw();
+  virtual int handle(int);
+
+private:
+  Fl_Window * parent;
+  std::string title;
+  bool initialized;
+};
+
+#endif

ias_robot.cpp

@@ -28,8 +28,11 @@
 
 #include "ias_robot.hpp"
 
+static const float  TURN_DEG_PER_SEC = 40.0f;
+static const float  METERS_PER_SEC   = 0.4f;
 static const long   HALF_SECOND_USEC = 500000;
 static const double MIN_DIST_M       = 1.5;
+static const double CRIT_DIST_M       = 1.0;
 
 IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port) : Robot(hostname, port) {
   std::cout << "Creating IAS-SS robot on host \"" << hostname << "\" and port " << port << "." << std::endl;
@@ -44,8 +47,6 @@ void IASSS_Robot::run() {
   long   then, now, delta, wait;
   struct timeval tv;
   double dist   = std::numeric_limits<double>::infinity();
-  double dist_l = 0.0;
-  double dist_r = 0.0;
 
   _p_client->Read();
   rv = gettimeofday(&tv, NULL);
@@ -57,21 +58,12 @@ void IASSS_Robot::run() {
   for(int i = 96; i < 126; i++)
     dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist;
 
-  if(dist < MIN_DIST_M) {
-    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);
-
-    if(dist_r >= dist_l)
-      _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-20));
-    else
-      _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(20));
+  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(0.4f, 0.0f);
+    _p_proxy->SetSpeed(METERS_PER_SEC, 0.0f);
   /******************************************************************************
    * WALL AVOIDANCE END                                                         *
    ******************************************************************************/
@@ -83,3 +75,21 @@ void IASSS_Robot::run() {
   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;
+
+  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);
+
+  if(dist_r >= dist_l)
+    _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(-turn_speed));
+  else
+    _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(turn_speed));
+}

ias_robot.hpp

@@ -45,6 +45,9 @@ public:
   virtual ~IASSS_Robot();
 
   virtual void run();
+
+private:
+  void avoid_wall(float front_speed, float turn_speed);
 };
 
 #endif

ias_ss.cpp

@@ -28,12 +28,17 @@
 #include <cstdlib>
 #include <csignal>
 
+#include "gui.hpp"
 #include "ias_robot.hpp"
 
-const uint32_t    PORT = PlayerCc::PLAYER_PORTNUM + 1;
-const uint32_t    NUM_ROBOTS = 4;
+const char * TITLE = "Pheromone map";
+const int W = 256;
+const int H = 256;
+const uint32_t PORT = PlayerCc::PLAYER_PORTNUM + 1;
+const uint32_t NUM_ROBOTS = 4;
 
 static bool done = false;
+static Fl_Window * window;
 
 extern "C" void handler(int signal) {
   done = true;
@@ -50,12 +55,20 @@ extern "C" void * robot_thread(void * arg) {
   return NULL;
 }
 
+void create_gui(int argc, char **argv) {
+  window = new Fl_Window(20, 40, W, H, TITLE);
+  new GlGui(window, 0, 0, W, H, TITLE);
+  window->end();
+  window->show(argc, argv);
+  window->make_current();
+}
+
 int main(int argc, char **argv) {
   pthread_t                  robot_threads[NUM_ROBOTS];
   std::vector<IASSS_Robot *> robots;
 
   signal(SIGINT, handler);
-  
+
   try {
     // Initialize the robot objects and threads.
     for(uint32_t i = 0; i < NUM_ROBOTS; ++i) {
@@ -67,6 +80,9 @@ int main(int argc, char **argv) {
       }
     }
 
+    create_gui(argc, argv);
+    Fl::run();
+    
     // Wait for all the robots to finish.
     for(uint32_t i = 0; i < NUM_ROBOTS; ++i) {
       if(pthread_join(robot_threads[i], NULL) != 0) {

robot.cpp

@@ -43,3 +43,7 @@ Robot::~Robot() {
   delete _r_proxy;
   delete _p_client;
 }
+
+void Robot::log(std::string msg) {
+  std::cout << "ROBOT(" << _host_name << ":" << _port << ") - " << msg << std::endl;
+}

robot.hpp

@@ -46,6 +46,8 @@ protected:
   PlayerCc::PlayerClient *    _p_client;
   PlayerCc::Position2dProxy * _p_proxy;
   PlayerCc::RangerProxy *     _r_proxy;
+
+  void log(std::string msg);
 };
 
 #endif