added map.

gazebo_sim/launch/gazebo_sim.launch

@@ -5,5 +5,5 @@
   </include>
   <!-- <include file="$(find qt_pi_gazebo_sim)/launch/depth_to_laser.launch"/> -->
   <include file="$(find qt_pi_gazebo_sim)/launch/static_tf.launch"/>
-
+  <include file="$(find qt_pi_gazebo_sim)/launch/move_base_amcl.launch"/>
-</launch>
+</launch>

qt_pi/CMakeLists.txt

@@ -48,15 +48,21 @@ include_directories(include
 add_library(ble_connect include/ble_connect/ble_connect.h src/ble_connect.c)
 target_link_libraries(ble_connect ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS})
 
-#ble_connect_test executable generation
-add_executable(ble_connect_test tests/ble_test.cpp)
-target_link_libraries(ble_connect_test ble_connect)
-
-
 #speller library generation
 add_library(speller ${SRCS} ${FORMS_HPP} ${MOC_HPP} ${RCC} ${HPP})
 target_link_libraries(speller Qt5::Widgets Qt5::Core Qt5::OpenGL Qt5::Gui ${catkin_LIBRARIES} ble_connect)
 
+
+#ble_connect_test executable generation
+add_executable(ble_connect_test tests/ble_test.cpp)
+target_link_libraries(ble_connect_test ble_connect)
+
+#QImage view test executable generation
+add_executable(map_load_test tests/map_load_test.cpp)
+target_link_libraries(map_load_test speller)
+
+
+
 #main executable generation
 add_executable(speller_ui src/main.cpp)
 target_link_libraries(speller_ui speller)

qt_pi/include/ble_connect/ble_connect.h

@@ -23,7 +23,7 @@ extern int insight_destroy();
 extern int insight_get_status();
 extern int insight_start_notif();
 extern int insight_stop_notif();
-extern void insight_write_to_buffer(int len, struct insight_data *buf);
+extern int insight_write_to_buffer(int len, struct insight_data *buf);
 extern int insight_buffer_status();
 extern int insight_stop_write_to_buffer();
 extern int insight_version();

qt_pi/include/mapview/mapview.h

@@ -7,6 +7,7 @@
 #include <QOpenGLBuffer>
 #include <QOpenGLFunctions_3_1>
 #include <QOpenGLShaderProgram>
+#include <QOpenGLTexture>
 #include <QOpenGLVertexArrayObject>
 #include <QOpenGLWindow>
 #include <QString>
@@ -37,33 +38,31 @@ protected:
   void teardownGL();
   void printContextInformation();
 
-signals:
-  void update_GUI();
-  void keyPress(QKeyEvent *key);
-
 private:
   QOpenGLBuffer m_vertex;
   QOpenGLVertexArrayObject m_object;
   QOpenGLShaderProgram *m_program;
 
+  QOpenGLBuffer map_vertex;
+  QOpenGLVertexArrayObject map_object;
+  QOpenGLShaderProgram *map_program;
+  QOpenGLTexture *map_texture;
+
   volatile int sleep_control = 1;
   volatile int blinking;
+  int map_redraw;
 
   std::thread *t;
 
   void run();
   void keyPressEvent(QKeyEvent *key) override;
-  //   QTimer *t1;
+
-  //   QTimer *t2;
+signals:
-  //   QTimer *t3;
+  void update_GUI();
-  //   QTimer *t4;
+  void keyPress(QKeyEvent *key);
-  //   int blinking = 0;
+  void map_loaded();
-  //   int state = 0;
+private slots:
-  // private slots:
+  void load_texture(int, Vertex *texture, QImage map_img);
-  //   void t1Slot();
-  //   void t2Slot();
-  //   void t3Slot();
-  //   void t4Slot();
 };
 
 #endif

qt_pi/include/mapview/vertex.h

@@ -1,31 +1,40 @@
 #ifndef VERTEX_H
 #define VERTEX_H
 
+#include <QVector2D>
 #include <QVector3D>
+#include <QVector4D>
 
 class Vertex {
 public:
   // Constructors
   Q_DECL_CONSTEXPR Vertex();
   Q_DECL_CONSTEXPR explicit Vertex(const QVector3D &position);
-  Q_DECL_CONSTEXPR Vertex(const QVector3D &position, const QVector3D &color);
+  Q_DECL_CONSTEXPR Vertex(const QVector3D &position, const QVector4D &color);
+  Q_DECL_CONSTEXPR Vertex(const QVector3D &position, const QVector4D &color,
+                          const QVector2D &texCoord);
 
   // Accessors / Mutators
   Q_DECL_CONSTEXPR const QVector3D &position() const;
-  Q_DECL_CONSTEXPR const QVector3D &color() const;
+  Q_DECL_CONSTEXPR const QVector4D &color() const;
+  Q_DECL_CONSTEXPR const QVector2D &texCoord() const;
   void setPosition(const QVector3D &position);
-  void setColor(const QVector3D &color);
+  void setColor(const QVector4D &color);
+  void setTexCoord(const QVector2D &texCoord);
 
   // OpenGL Helpers
   static const int PositionTupleSize = 3;
-  static const int ColorTupleSize = 3;
+  static const int ColorTupleSize = 4;
+  static const int TexCoordTupleSize = 2;
   static Q_DECL_CONSTEXPR int positionOffset();
   static Q_DECL_CONSTEXPR int colorOffset();
+  static Q_DECL_CONSTEXPR int texCoordOffset();
   static Q_DECL_CONSTEXPR int stride();
 
 private:
   QVector3D m_position;
-  QVector3D m_color;
+  QVector4D m_color;
+  QVector2D m_texCoord;
 };
 
 /*******************************************************************************
@@ -40,21 +49,30 @@ Q_DECL_CONSTEXPR inline Vertex::Vertex() {}
 Q_DECL_CONSTEXPR inline Vertex::Vertex(const QVector3D &position)
     : m_position(position) {}
 Q_DECL_CONSTEXPR inline Vertex::Vertex(const QVector3D &position,
-                                       const QVector3D &color)
+                                       const QVector4D &color)
     : m_position(position), m_color(color) {}
+Q_DECL_CONSTEXPR inline Vertex::Vertex(const QVector3D &position,
+                                       const QVector4D &color,
+                                       const QVector2D &texCoord)
+    : m_position(position), m_color(color), m_texCoord(texCoord) {}
 
 // Accessors / Mutators
 Q_DECL_CONSTEXPR inline const QVector3D &Vertex::position() const {
   return m_position;
 }
-Q_DECL_CONSTEXPR inline const QVector3D &Vertex::color() const {
+Q_DECL_CONSTEXPR inline const QVector4D &Vertex::color() const {
   return m_color;
 }
+Q_DECL_CONSTEXPR inline const QVector2D &Vertex::texCoord() const {
+  return m_texCoord;
+}
 void inline Vertex::setPosition(const QVector3D &position) {
   m_position = position;
 }
-void inline Vertex::setColor(const QVector3D &color) { m_color = color; }
+void inline Vertex::setColor(const QVector4D &color) { m_color = color; }
-
+void inline Vertex::setTexCoord(const QVector2D &texCoord) {
+  m_texCoord = texCoord;
+}
 // OpenGL Helpers
 Q_DECL_CONSTEXPR inline int Vertex::positionOffset() {
   return offsetof(Vertex, m_position);
@@ -62,6 +80,9 @@ Q_DECL_CONSTEXPR inline int Vertex::positionOffset() {
 Q_DECL_CONSTEXPR inline int Vertex::colorOffset() {
   return offsetof(Vertex, m_color);
 }
+Q_DECL_CONSTEXPR inline int Vertex::texCoordOffset() {
+  return offsetof(Vertex, m_texCoord);
+}
 Q_DECL_CONSTEXPR inline int Vertex::stride() { return sizeof(Vertex); }
 
 #endif // VERTEX_H

qt_pi/include/speller/speller.h

@@ -1,6 +1,7 @@
 #ifndef SPELLER
 #define SPELLER
 
+#include "QLabel"
 #include "ble_connect/ble_connect.h"
 #include "mapview/mapview.h"
 #include <QKeyEvent>
@@ -10,6 +11,9 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Quaternion.h>
 #include <map>
+#include <nav_msgs/GetMap.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
 
 class Speller : public QThread {
@@ -25,10 +29,37 @@ private:
   int argc;
   char **argv;
   void run() override;
+
+  enum Map_Type { SQUARE, HORIZONTAL, VERTICAL };
+  int in_view;
+
+  ros::NodeHandle *nh;
+  ros::Publisher pub_goal;
+  ros::Subscriber sub_pose;
+  ros::ServiceClient map_client;
+
+  nav_msgs::OccupancyGrid map;
+  geometry_msgs::PoseStamped pose;
+  geometry_msgs::Pose pose_current;
+
+  struct insight_data *buf;
+  int buf_len = 1000;
+
+  uint8_t *map_tex;
+  volatile int map_loading_done;
+
+  volatile int spelling;
+
+  void poseCB(nav_msgs::Odometry::ConstPtr &msg);
+  __inline__ void init_ble();
+  __inline__ void get_ble_data();
+  __inline__ void init_ros();
+  __inline__ void load_map();
+
 private slots:
   void keyEvent(QKeyEvent *key);
-
+  void map_loaded();
-private:
+signals:
-  volatile int spelling;
+  void load_texture(int, Vertex *, QImage);
 };
 #endif

qt_pi/resources/resources.qrc

@@ -1,6 +1,9 @@
 <RCC>
     <qresource prefix="/">
-        <file>shaders/simple.frag</file>
+        <file>shaders/blink.frag</file>
-        <file>shaders/simple.vert</file>
+        <file>shaders/blink.vert</file>
+        <file>shaders/map.vert</file>
+        <file>shaders/map.frag</file>
+        <file>playground.pgm</file>
     </qresource>
 </RCC>

qt_pi/resources/shaders/blink.vert

@@ -1,10 +1,10 @@
 #version 330
 layout(location = 0) in vec3 position;
-layout(location = 1) in vec3 color;
+layout(location = 1) in vec4 color;
 out vec4 vColor;
 
 void main()
 {
   gl_Position = vec4(position, 1.0);
-  vColor = vec4(color, 1.0);
+  vColor = color;
 }

qt_pi/resources/shaders/map.frag

@@ -0,0 +1,13 @@
+#version 330 core
+out vec4 FragColor;
+
+in vec4 ourColor;
+in vec2 TexCoord;
+
+uniform sampler2D ourTexture;
+
+void main()
+{
+    //FragColor = ourColor;
+    FragColor = texture(ourTexture, TexCoord);
+}

qt_pi/resources/shaders/map.vert

@@ -0,0 +1,14 @@
+#version 330 core
+layout (location = 0) in vec3 aPos;
+layout (location = 1) in vec3 aColor;
+layout (location = 2) in vec2 aTexCoord;
+
+out vec4 ourColor;
+out vec2 TexCoord;
+
+void main()
+{
+    gl_Position = vec4(aPos, 1.0);
+    ourColor = vec4(aColor, 1.0);
+    TexCoord = aTexCoord;
+}

qt_pi/src/ble_connect.c

@@ -64,11 +64,14 @@ void notification_handler(const uuid_t *uuid, const uint8_t *val,
     write_to_buffer = 2;
 }
 
-void insight_write_to_buffer(int len, struct insight_data *buf) {
+int insight_write_to_buffer(int len, struct insight_data *buf) {
+  if (status != 2)
+    return status;
   data_buffer = buf;
   data_buffer_length = len;
   data_index = 0;
   write_to_buffer = 1;
+  return 0;
 }
 
 int insight_stop_write_to_buffer() {

qt_pi/src/main.cpp

@@ -16,7 +16,7 @@ int main(int argc, char *argv[]) {
   Speller spell(argc, argv);
   spell.window->setFormat(format);
   spell.window->show();
-  // spell.start();
+  spell.start();
 
   return app.exec();
 }

qt_pi/src/mapview.cpp

@@ -5,6 +5,7 @@ MapView::MapView(QWidget *parent) {
 
   blinking = 0;
   count = 4;
+  map_redraw = -1;
 }
 
 MapView::~MapView() {
@@ -13,12 +14,52 @@ MapView::~MapView() {
   teardownGL();
 }
 
+void MapView::run() {
+  usleep(500000);
+
+  uint fps = 0;
+  uint sq_state = 0;
+  uint pre_sq_state = 0;
+
+  while (blinking) {
+
+    auto time = std::chrono::high_resolution_clock::now();
+
+    count = 0;
+    int changed = 0;
+
+    fps++;
+
+    for (int i = 0; i < 4; i++) {
+      if (!(fps % (i + 4))) {
+        sq_state = sq_state ^ (0b1 << i);
+        changed = 1;
+        startingElements[count] = i * 4 + ((sq_state >> i) & 0b1) * 16;
+        count++;
+      }
+    }
+
+    if (changed) {
+
+      sleep_control = 0;
+      emit(update_GUI());
+      while (!sleep_control)
+        sleep(0);
+
+      pre_sq_state = sq_state;
+    }
+
+    usleep(16665 - std::chrono::duration_cast<std::chrono::microseconds>(
+                       std::chrono::high_resolution_clock::now() - time)
+                       .count());
+  }
+}
+
 void MapView::stop_blinking() {
   if (!blinking)
     return;
-
   blinking = 0;
-
+  sleep_control = 1;
   t->join();
   free(t);
 }
@@ -31,6 +72,7 @@ void MapView::toggle_blinking() {
 }
 
 void MapView::start_blinking() {
+
   if (blinking)
     return;
   blinking = 1;
@@ -38,64 +80,58 @@ void MapView::start_blinking() {
   sleep(0);
 }
 
-void MapView::run() {
-  usleep(500000);
-  uint fps = 0;
-  uint sq_state = 0;
-  while (blinking) {
-
-    auto time = std::chrono::high_resolution_clock::now();
-    if (!(fps % 4))
-      sq_state = sq_state ^ (0b00000001);
-    if (!(fps % 5))
-      sq_state = sq_state ^ (0b00000010);
-    if (!(fps % 6))
-      sq_state = sq_state ^ (0b00000100);
-    if (!(fps % 7))
-      sq_state = sq_state ^ (0b00001000);
-
-    count = 0;
-    for (GLint i = 0; i < 4; i++) {
-      if ((sq_state >> i) & 0b1) {
-        startingElements[count] = i * 4;
-        count++;
-      }
-    }
-    fps++;
-
-    emit(update_GUI());
-    while (!sleep_control)
-      sleep(0);
-    sleep_control = 0;
-
-    usleep(16665 - std::chrono::duration_cast<std::chrono::microseconds>(
-                       std::chrono::high_resolution_clock::now() - time)
-                       .count());
-  }
-}
-
 static const Vertex sg_vertexes[] = {
-    Vertex(QVector3D(-0.55f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.55f, 0.55f, 0.0f),
-    Vertex(QVector3D(-0.45f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
-    Vertex(QVector3D(-0.45f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.45f, 0.55f, 0.0f),
-    Vertex(QVector3D(-0.55f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
+    Vertex(QVector3D(-0.45f, 0.45f, 0.0f),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
+    Vertex(QVector3D(-0.55f, 0.45f, 0.0f),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
 
-    Vertex(QVector3D(-0.55f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.55f, -0.55f, 0.0f),
-    Vertex(QVector3D(-0.45f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
-    Vertex(QVector3D(-0.45f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.45f, -0.55f, 0.0f),
-    Vertex(QVector3D(-0.55f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
+    Vertex(QVector3D(-0.45f, -0.45f, 0.0f),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
+    Vertex(QVector3D(-0.55f, -0.45f, 0.0f),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
 
-    Vertex(QVector3D(0.55f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.55f, 0.55f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
-    Vertex(QVector3D(0.45f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.45f, 0.55f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
-    Vertex(QVector3D(0.45f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.45f, 0.45f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
-    Vertex(QVector3D(0.55f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.55f, 0.45f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
 
-    Vertex(QVector3D(0.55f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.55f, -0.55f, 0.0f),
-    Vertex(QVector3D(0.45f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
-    Vertex(QVector3D(0.45f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.45f, -0.55f, 0.0f),
-    Vertex(QVector3D(0.55f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
+    Vertex(QVector3D(0.45f, -0.45f, 0.0f),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
+    Vertex(QVector3D(0.55f, -0.45f, 0.0f),
+           QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
 
-};
+    Vertex(QVector3D(-0.55f, 0.55f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(-0.45f, 0.55f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(-0.45f, 0.45f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(-0.55f, 0.45f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
+
+    Vertex(QVector3D(-0.55f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(-0.45f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(-0.45f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(-0.55f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+
+    Vertex(QVector3D(0.55f, 0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(0.45f, 0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(0.45f, 0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(0.55f, 0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+
+    Vertex(QVector3D(0.55f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(0.45f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(0.45f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
+    Vertex(QVector3D(0.55f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f))};
 
 void MapView::initializeGL() {
   initializeOpenGLFunctions();
@@ -104,11 +140,16 @@ void MapView::initializeGL() {
   glClearColor(0, 0, 0, 1);
   {
     // Create Shader (Do not release until VAO is created)
+    map_program = new QOpenGLShaderProgram();
+    map_program->addShaderFromSourceFile(QOpenGLShader::Vertex,
+                                         ":/shaders/map.vert");
+    map_program->addShaderFromSourceFile(QOpenGLShader::Fragment,
+                                         ":/shaders/map.frag");
     m_program = new QOpenGLShaderProgram();
     m_program->addShaderFromSourceFile(QOpenGLShader::Vertex,
-                                       ":/shaders/simple.vert");
+                                       ":/shaders/blink.vert");
     m_program->addShaderFromSourceFile(QOpenGLShader::Fragment,
-                                       ":/shaders/simple.frag");
+                                       ":/shaders/blink.frag");
     m_program->link();
     m_program->bind();
 
@@ -136,7 +177,8 @@ void MapView::initializeGL() {
 }
 
 void MapView::paintGL() {
-  glClear(GL_COLOR_BUFFER_BIT);
+  // glClear(GL_COLOR_BUFFER_BIT);
+
   m_program->bind();
   {
     m_object.bind();
@@ -144,10 +186,25 @@ void MapView::paintGL() {
     m_object.release();
   }
   m_program->release();
+
+  if (map_redraw == 1) {
+    map_program->bind();
+    map_object.bind();
+    map_texture->bind();
+    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+    map_texture->release();
+    map_object.release();
+    map_program->release();
+    map_redraw = 0;
+  }
+
   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();
@@ -187,4 +244,47 @@ void MapView::teardownGL() {
   delete m_program;
 }
 
+void MapView::load_texture(int len, Vertex *textures, 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_vertex.bind();
+  map_vertex.setUsagePattern(QOpenGLBuffer::StaticDraw);
+  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)); }

qt_pi/src/speller.cpp

@@ -1,69 +1,137 @@
 #include "speller/speller.h"
-
 Speller::Speller(int ar, char **av) {
+
   argc = ar;
   argv = av;
   spelling = 0;
+
   window = new MapView();
+
   connect(window, SIGNAL(keyPress(QKeyEvent *)), this,
           SLOT(keyEvent(QKeyEvent *)));
-
+  connect(this, SIGNAL(load_texture(int, Vertex *, QImage)), window,
-  const char *a = "EF:BD:39:4E:08:49";
+          SLOT(load_texture(int, Vertex *, QImage)));
-  const char *b = "81072f41-9f3d-11e3-a9dc-0002a5d5c51b";
+  connect(window, SIGNAL(map_loaded()), this, SLOT(map_loaded()));
-  insight_init(a, b);
 }
 
 Speller::~Speller() {
+
   insight_stop_notif();
   insight_destroy();
-  this->terminate();
+  ros::shutdown();
+  window->~MapView();
+  this->wait();
 }
 
 void Speller::run() {
 
-  const std::string node_name = "speller";
+  // init_ble();
-
+  init_ros();
-  ros::init(argc, argv, node_name);
+  load_map();
-  ros::NodeHandle nh;
+  ros::Rate loop(10);
-  ros::Publisher pub =
+  ros::Rate loop2(10);
-      nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
-  geometry_msgs::PoseStamped msg;
-  geometry_msgs::Pose pose;
-  geometry_msgs::Point point;
-  geometry_msgs::Quaternion quat;
-  pose.position = point;
-  pose.orientation = quat;
-  msg.pose = pose;
-  point.x = 0;
-  point.y = 0;
-  point.z = 0;
-  quat.w = 1;
-  quat.x = 0;
-  quat.y = 0;
-  quat.z = 0;
-
   while (ros::ok()) {
-    while (spelling) {
+    loop2.sleep();
-      int len = 1000;
+    while (spelling && ros::ok()) {
-      struct insight_data *buf =
+      get_ble_data();
-          (struct insight_data *)malloc(sizeof(struct insight_data) * len);
+      loop.sleep();
-
-      window->start_blinking();
-      insight_write_to_buffer(len, buf);
-
-      while (insight_buffer_status() == 1)
-        sleep(0);
-
-      window->stop_blinking();
-
       // TODO-Send data for classification
       // int a; if((a = classify(len, buf))) window->zoom(a);
     }
   }
 }
 
+__inline__ void Speller::load_map() {
+
+  in_view = -1;
+
+  if (map_tex != NULL)
+    free(map_tex);
+
+  map_tex = (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[3 * i + 0] = 0;
+      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;
+
+  QImage tmp(map_tex, map.info.width, map.info.height, 3 * map.info.width,
+             QImage::Format_RGB888);
+
+  Vertex tmp_info[] = {
+      Vertex(QVector3D(-1.0f, -1.0f, -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),
+             QVector2D(0.0f, 1.0f)),
+      Vertex(QVector3D(1.0f, 1.0f, -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),
+             QVector2D(1.0f, 0.0f))};
+
+  emit(load_texture(sizeof(tmp_info), tmp_info, tmp));
+
+  map_loading_done = 0;
+
+  while (!map_loading_done)
+    sleep(0);
+}
+
+__inline__ void Speller::get_ble_data() {
+
+  window->start_blinking();
+  int ret = insight_write_to_buffer(buf_len, buf);
+  sleep(10);
+  while ((!ret) && (insight_buffer_status() == 1))
+    sleep(0);
+
+  window->stop_blinking();
+}
+
+__inline__ void Speller::init_ros() {
+
+  ros::init(argc, argv, "speller");
+
+  nh = new ros::NodeHandle();
+  pub_goal =
+      nh->advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
+  map_client = nh->serviceClient<nav_msgs::GetMap>("/static_map");
+
+  //  sub_pose = nh->subscribe("qt_pi/pose", 1, &Speller::poseCB, this);
+
+  nav_msgs::GetMap tmp;
+  while (!map_client.call(tmp))
+    sleep(0);
+  map = tmp.response.map;
+}
+
+void Speller::poseCB(nav_msgs::Odometry::ConstPtr &msg) {
+  pose_current = msg->pose.pose;
+}
+
+__inline__ void Speller::init_ble() {
+
+  const char *a = "EF:BD:39:4E:08:49";
+  const char *b = "81072f41-9f3d-11e3-a9dc-0002a5d5c51b";
+
+  if (!insight_init(a, b))
+    buf = (struct insight_data *)malloc(sizeof(struct insight_data) * buf_len);
+}
+
+void Speller::map_loaded() { map_loading_done = 1; }
+
 void Speller::keyEvent(QKeyEvent *e) {
+
   if (e->key() == Qt::Key_Space) {
+
     if (!spelling)
       insight_start_notif();
     else

qt_pi/tests/map_load_test.cpp

@@ -0,0 +1,43 @@
+#include <QApplication>
+#include <QImage>
+#include <QLabel>
+#include <nav_msgs/GetMap.h>
+#include <ros/ros.h>
+
+int main(int argc, char *argv[]) {
+  QApplication a(argc, argv);
+  ros::init(argc, argv, "Holla");
+
+  QImage *myImage;
+
+  auto nh = new ros::NodeHandle();
+
+  auto map_client = nh->serviceClient<nav_msgs::GetMap>("/static_map");
+
+  nav_msgs::GetMap tmp;
+  while (!map_client.call(tmp))
+    sleep(0);
+  auto map = tmp.response.map;
+
+  uint8_t *map_tex = (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[3 * i + 0] = 0;
+      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;
+    }
+  }
+  QImage *tmp_ = (new QImage(map_tex, map.info.width, map.info.height,
+                             3 * map.info.width, QImage::Format_RGB888));
+  QLabel myLabel;
+  myLabel.setPixmap(QPixmap::fromImage(tmp_->mirrored()));
+
+  myLabel.show();
+
+  return a.exec();
+}