Select and zoom into map done

qt_pi/include/mapview/mapview.h

@@ -47,6 +47,8 @@ private:
   QOpenGLVertexArrayObject map_object;
   QOpenGLShaderProgram *map_program;
   QOpenGLTexture *map_texture;
+  Vertex *vertices;
+  int vertices_len;
 
   volatile int sleep_control = 1;
   volatile int blinking;
@@ -56,13 +58,14 @@ private:
 
   void run();
   void keyPressEvent(QKeyEvent *key) override;
-
+  void mousePressEvent(QMouseEvent *event) override;
 signals:
   void update_GUI();
   void keyPress(QKeyEvent *key);
   void map_loaded();
+  void mouse_press(QMouseEvent *);
 private slots:
-  void load_texture(int, Vertex *texture, QImage map_img);
+  void load_texture(int, Vertex *, QImage *);
 };
 
 #endif

qt_pi/include/speller/speller.h

@@ -1,7 +1,6 @@
 #ifndef SPELLER
 #define SPELLER
 
-#include "QLabel"
 #include "ble_connect/ble_connect.h"
 #include "mapview/mapview.h"
 #include <QKeyEvent>
@@ -30,8 +29,14 @@ private:
   char **argv;
   void run() override;
 
-  enum Map_Type { SQUARE, HORIZONTAL, VERTICAL };
+  enum Map_Shape { SQUARE, HORIZONTAL, VERTICAL };
-  int in_view;
+  enum Classify_Type { TOP_RIGHT = 1, TOP_LEFT, BOTTOM_LEFT, BOTTOM_RIGHT };
+
+  QImage *curr_map;
+  QRect *area_displayed;
+  uint8_t *map_tex_buffer;
+  int map_shape;
+  int classifier_result;
 
   ros::NodeHandle *nh;
   ros::Publisher pub_goal;
@@ -43,23 +48,25 @@ private:
   geometry_msgs::Pose pose_current;
 
   struct insight_data *buf;
-  int buf_len = 1000;
+  int buf_len = 100;
 
-  uint8_t *map_tex;
   volatile int map_loading_done;
 
   volatile int spelling;
-
+  void reconfigure_map(int, int);
   void poseCB(nav_msgs::Odometry::ConstPtr &msg);
   __inline__ void init_ble();
   __inline__ void get_ble_data();
   __inline__ void init_ros();
   __inline__ void load_map();
 
+  void classify(int len, struct insight_data *buf);
+  QPoint me;
 private slots:
   void keyEvent(QKeyEvent *key);
   void map_loaded();
+  void mouse_press(QMouseEvent *);
 signals:
-  void load_texture(int, Vertex *, QImage);
+  void load_texture(int, Vertex *, QImage *);
 };
 #endif

qt_pi/src/mapview.cpp

@@ -3,9 +3,14 @@
 MapView::MapView(QWidget *parent) {
   connect(this, SIGNAL(update_GUI()), this, SLOT(update()));
 
+  vertices_len = 144;
+  vertices = (Vertex *)malloc(vertices_len);
+  memset(vertices, 0, vertices_len);
+
+  map_texture = NULL;
   blinking = 0;
-  count = 4;
+  count = 0;
-  map_redraw = -1;
+  map_redraw = 0;
 }
 
 MapView::~MapView() {
@@ -34,8 +39,8 @@ void MapView::run() {
       if (!(fps % (i + 4))) {
         sq_state = sq_state ^ (0b1 << i);
         changed = 1;
-        startingElements[count] = i * 4 + ((sq_state >> i) & 0b1) * 16;
+        if (((sq_state >> i) & 0b1))
-        count++;
+          startingElements[count++] = i * 4;
       }
     }
 
@@ -59,6 +64,7 @@ void MapView::stop_blinking() {
   if (!blinking)
     return;
   blinking = 0;
+  count = 0;
   sleep_control = 1;
   t->join();
   free(t);
@@ -145,6 +151,35 @@ void MapView::initializeGL() {
                                          ":/shaders/map.vert");
     map_program->addShaderFromSourceFile(QOpenGLShader::Fragment,
                                          ":/shaders/map.frag");
+    map_program->link();
+    map_program->bind();
+    map_vertex.create();
+
+    map_vertex.bind();
+    map_vertex.setUsagePattern(QOpenGLBuffer::DynamicDraw);
+    map_vertex.allocate(vertices, vertices_len);
+    // Create Vertex Array Object
+    map_object.create();
+    map_object.bind();
+    map_program->enableAttributeArray(0);
+    map_program->enableAttributeArray(1);
+    map_program->enableAttributeArray(2);
+
+    map_program->setAttributeBuffer(0, GL_FLOAT, Vertex::positionOffset(),
+                                    Vertex::PositionTupleSize,
+                                    Vertex::stride());
+    map_program->setAttributeBuffer(1, GL_FLOAT, Vertex::colorOffset(),
+                                    Vertex::ColorTupleSize, Vertex::stride());
+    map_program->setAttributeBuffer(2, GL_FLOAT, Vertex::texCoordOffset(),
+                                    Vertex::TexCoordTupleSize,
+                                    Vertex::stride());
+
+    // map_program->setUniformValue("ourTexture", 0);
+
+    map_object.release();
+    map_vertex.release();
+    map_program->release();
+
     m_program = new QOpenGLShaderProgram();
     m_program->addShaderFromSourceFile(QOpenGLShader::Vertex,
                                        ":/shaders/blink.vert");
@@ -177,15 +212,8 @@ void MapView::initializeGL() {
 }
 
 void MapView::paintGL() {
-  // glClear(GL_COLOR_BUFFER_BIT);
 
-  m_program->bind();
+  glClear(GL_COLOR_BUFFER_BIT);
-  {
-    m_object.bind();
-    glMultiDrawArrays(GL_TRIANGLE_FAN, startingElements, counts, count);
-    m_object.release();
-  }
-  m_program->release();
 
   if (map_redraw == 1) {
     map_program->bind();
@@ -195,16 +223,21 @@ void MapView::paintGL() {
     map_texture->release();
     map_object.release();
     map_program->release();
-    map_redraw = 0;
   }
 
+  m_program->bind();
+  {
+    m_object.bind();
+    glMultiDrawArrays(GL_TRIANGLE_FAN, startingElements, counts, count);
+    m_object.release();
+  }
+  m_program->release();
+
   sleep_control = 1;
 }
 
 void MapView::resizeGL(int w, int h) {
 
-  if (map_redraw == 0)
-    map_redraw = 1;
   glViewport(0, 0, w, h);
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
@@ -239,52 +272,29 @@ void MapView::printContextInformation() {
 }
 
 void MapView::teardownGL() {
+  map_object.destroy();
+  map_vertex.destroy();
+  delete map_program;
   m_object.destroy();
   m_vertex.destroy();
   delete m_program;
 }
 
-void MapView::load_texture(int len, Vertex *textures, QImage map_img) {
+void MapView::load_texture(int len, Vertex *ver, QImage *map_img) {
-
-  map_program->link();
-  map_program->bind();
-
-  map_texture = new QOpenGLTexture(map_img);
-  //
-  // map_texture->setMinificationFilter(QOpenGLTexture::LinearMipMapLinear);
-  // map_texture->setMagnificationFilter(QOpenGLTexture::Linear);
-  // Create Buffer (Do not release until VAO is created)
-
-  map_vertex.create();
 
+  map_redraw = 0;
+  if (map_texture != NULL)
+    map_texture->destroy();
+  map_texture = new QOpenGLTexture(*map_img);
+  vertices_len = len;
+  memcpy(vertices, ver, len);
   map_vertex.bind();
-  map_vertex.setUsagePattern(QOpenGLBuffer::StaticDraw);
+  map_vertex.write(0, vertices, vertices_len);
-  map_vertex.allocate(textures, len);
-  // Create Vertex Array Object
-  map_object.create();
-  map_object.bind();
-  map_program->enableAttributeArray(0);
-  map_program->enableAttributeArray(1);
-  map_program->enableAttributeArray(2);
-
-  map_program->setAttributeBuffer(0, GL_FLOAT, Vertex::positionOffset(),
-                                  Vertex::PositionTupleSize, Vertex::stride());
-  map_program->setAttributeBuffer(1, GL_FLOAT, Vertex::colorOffset(),
-                                  Vertex::ColorTupleSize, Vertex::stride());
-  map_program->setAttributeBuffer(2, GL_FLOAT, Vertex::texCoordOffset(),
-                                  Vertex::TexCoordTupleSize, Vertex::stride());
-
-  // map_program->setUniformValue("ourTexture", 0);
-
-  map_object.release();
-  map_vertex.release();
-  map_program->release();
-
   emit(map_loaded());
 
   map_redraw = 1;
-
   this->update();
 }
 
 void MapView::keyPressEvent(QKeyEvent *key) { emit(keyPress(key)); }
+void MapView::mousePressEvent(QMouseEvent *event) { emit(mouse_press(event)); }

qt_pi/src/speller.cpp

@@ -9,9 +9,11 @@ Speller::Speller(int ar, char **av) {
 
   connect(window, SIGNAL(keyPress(QKeyEvent *)), this,
           SLOT(keyEvent(QKeyEvent *)));
-  connect(this, SIGNAL(load_texture(int, Vertex *, QImage)), window,
+  connect(this, SIGNAL(load_texture(int, Vertex *, QImage *)), window,
-          SLOT(load_texture(int, Vertex *, QImage)));
+          SLOT(load_texture(int, Vertex *, QImage *)));
   connect(window, SIGNAL(map_loaded()), this, SLOT(map_loaded()));
+  connect(window, SIGNAL(mouse_press(QMouseEvent *)), this,
+          SLOT(mouse_press(QMouseEvent *)));
 }
 
 Speller::~Speller() {
@@ -19,7 +21,7 @@ Speller::~Speller() {
   insight_stop_notif();
   insight_destroy();
   ros::shutdown();
-  window->~MapView();
+  delete window;
   this->wait();
 }
 
@@ -33,51 +35,112 @@ void Speller::run() {
   while (ros::ok()) {
     loop2.sleep();
     while (spelling && ros::ok()) {
+      printf("Shape %d\n", map_shape);
       get_ble_data();
-      loop.sleep();
       // TODO-Send data for classification
-      // int a; if((a = classify(len, buf))) window->zoom(a);
+      // check classifier result();
+      classify(buf_len, buf);
+      printf("classifier %d\n", classifier_result);
+      reconfigure_map(classifier_result, map_shape);
+      load_map();
+      loop.sleep();
     }
   }
 }
 
-__inline__ void Speller::load_map() {
+void Speller::classify(int len, struct insight_data *buf) {
-
+  sleep(2);
-  in_view = -1;
+  if ((me.x() * me.y()) < 0) {
-
+    if (me.x() < 0)
-  if (map_tex != NULL)
+      classifier_result = TOP_LEFT;
-    free(map_tex);
+    else
-
+      classifier_result = BOTTOM_RIGHT;
-  map_tex = (uint8_t *)malloc(map.info.height * map.info.width * 3);
+  } else {
-
+    if (me.x() < 0)
-  for (int i = 0; i < map.info.height * map.info.width; i++) {
+      classifier_result = BOTTOM_LEFT;
-    if (map.data[i] > 50) {
+    else
-      map_tex[3 * i + 0] = 0;
+      classifier_result = TOP_RIGHT;
-      map_tex[3 * i + 1] = 0;
-      map_tex[3 * i + 2] = 0;
-    } else {
-      map_tex[3 * i + 0] = 255;
-      map_tex[3 * i + 1] = 255;
-      map_tex[3 * i + 2] = 255;
-    }
   }
+}
 
-  map_loading_done = 0;
+void Speller::reconfigure_map(int result, int shape) {
+  int w = area_displayed->width();
+  int h = area_displayed->height();
+  switch (shape) {
+  case SQUARE:
+    switch (result) {
+    case TOP_LEFT:
+      area_displayed->setWidth(w / 1.9);
+      area_displayed->setY(h * 0.9 / 1.9);
+      break;
+    case TOP_RIGHT:
+      area_displayed->setX(w * 0.9 / 1.9);
+      area_displayed->setY(h * 0.9 / 1.9);
+      break;
+    case BOTTOM_LEFT:
+      area_displayed->setHeight(h / 1.9);
+      area_displayed->setWidth(w / 1.9);
+      break;
+    case BOTTOM_RIGHT:
+      area_displayed->setHeight(h / 1.9);
+      area_displayed->setX(w * 0.9 / 1.9);
+      break;
+    }
+    break;
+  case HORIZONTAL:
+    switch (result) {
+    case TOP_LEFT:
+    case BOTTOM_LEFT:
+      area_displayed->setWidth(w / 1.9);
+      break;
+    case TOP_RIGHT:
+    case BOTTOM_RIGHT:
+      area_displayed->setX(w * 0.9 / 1.9);
+      break;
+    }
+    break;
+  case VERTICAL:
+    switch (result) {
+    case TOP_LEFT:
+    case TOP_RIGHT:
+      area_displayed->setY(h * 0.9 / 1.9);
+      break;
+    case BOTTOM_LEFT:
+    case BOTTOM_RIGHT:
+      area_displayed->setHeight(h / 1.9);
+      break;
+    }
+    break;
+  }
+}
 
-  QImage tmp(map_tex, map.info.width, map.info.height, 3 * map.info.width,
+__inline__ void Speller::load_map() {
-             QImage::Format_RGB888);
+  float max = std::max(area_displayed->width(), area_displayed->height());
+
+  float w = area_displayed->width() / max;
+  float h = area_displayed->height() / max;
+  float aspect_ratio = w / h;
+
+  if (aspect_ratio > 1.5)
+    map_shape = HORIZONTAL;
+  else if (aspect_ratio < 0.5)
+    map_shape = VERTICAL;
+  else
+    map_shape = SQUARE;
 
   Vertex tmp_info[] = {
-      Vertex(QVector3D(-1.0f, -1.0f, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
+      Vertex(QVector3D(-w, -h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
              QVector2D(0.0f, 0.0f)),
-      Vertex(QVector3D(-1.0f, 1.0f, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
+      Vertex(QVector3D(-w, h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
              QVector2D(0.0f, 1.0f)),
-      Vertex(QVector3D(1.0f, 1.0f, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
+      Vertex(QVector3D(w, h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
              QVector2D(1.0, 1.0f)),
-      Vertex(QVector3D(1.0f, -1.0f, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
+      Vertex(QVector3D(w, -h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
              QVector2D(1.0f, 0.0f))};
 
-  emit(load_texture(sizeof(tmp_info), tmp_info, tmp));
+  QImage tmp = curr_map->copy(*area_displayed);
+
+  emit(load_texture(sizeof(tmp_info), tmp_info, &tmp));
 
   map_loading_done = 0;
 
@@ -88,11 +151,11 @@ __inline__ void Speller::load_map() {
 __inline__ void Speller::get_ble_data() {
 
   window->start_blinking();
-  int ret = insight_write_to_buffer(buf_len, buf);
+  // int ret = insight_write_to_buffer(buf_len, buf);
-  sleep(10);
-  while ((!ret) && (insight_buffer_status() == 1))
-    sleep(0);
 
+  // while ((!ret) && (insight_buffer_status() == 1))
+  //    sleep(0);
+  sleep(2);
   window->stop_blinking();
 }
 
@@ -111,6 +174,24 @@ __inline__ void Speller::init_ros() {
   while (!map_client.call(tmp))
     sleep(0);
   map = tmp.response.map;
+
+  map_tex_buffer = (uint8_t *)malloc(map.info.height * map.info.width * 3);
+
+  for (int i = 0; i < map.info.height * map.info.width; i++) {
+    if (map.data[i] > 50) {
+      map_tex_buffer[3 * i + 0] = 0;
+      map_tex_buffer[3 * i + 1] = 0;
+      map_tex_buffer[3 * i + 2] = 0;
+    } else {
+      map_tex_buffer[3 * i + 0] = 255;
+      map_tex_buffer[3 * i + 1] = 255;
+      map_tex_buffer[3 * i + 2] = 255;
+    }
+  }
+  curr_map = new QImage(map_tex_buffer, map.info.width, map.info.height,
+                        3 * map.info.width, QImage::Format_RGB888);
+  area_displayed = new QRect(QPoint(0, 0), QSize(curr_map->size().height() / 2,
+                                                 curr_map->size().width()));
 }
 
 void Speller::poseCB(nav_msgs::Odometry::ConstPtr &msg) {
@@ -139,3 +220,10 @@ void Speller::keyEvent(QKeyEvent *e) {
     spelling = !spelling;
   }
 }
+
+void Speller::mouse_press(QMouseEvent *e) {
+  me = e->pos();
+  me.setX(me.x() - 500);
+  me.setY(500 - me.y());
+  printf("%d %d\n", me.x(), me.y());
+}