added blinking squares

qt_pi/CMakeLists.txt

@@ -3,7 +3,25 @@ project(qt_pi)
 
 set(CMAKE_CXX_FLAGS "-fpermissive -std=c++0x")
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+qt_build
+roscpp
+roslib
+)
+
+find_package(Qt5 REQUIRED COMPONENTS Core Gui OpenGL)
+
+
+#set(FORMS ui/speller.ui)
+set(MOC include/mapview/mapview.h)
+set(HPP include/speller/speller.h include/mapview/vertex.h)
+set(SRCS src/speller.cpp src/mapview.cpp )
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+#qt5_wrap_ui(FORMS_HPP ${FORMS})
+qt5_wrap_cpp(MOC_HPP ${MOC})
+qt5_add_resources(RCC resources/resources.qrc)
 
 catkin_python_setup()
 catkin_package(DEPENDS)
@@ -17,18 +35,8 @@ find_path(Mcrypt_INCLUDE_DIR mcrypt.h PATHS
 set(Mcrypt_LIB_PATHS /usr/lib)
 find_library(Mcrypt_LIBS NAMES mcrypt rtfilter PATHS ${Mcrypt_LIB_PATHS})
 
-include_directories(include)
+include_directories(include
-
+${catkin_INCLUDE_DIRS}
-install(DIRECTORY config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY nodes
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
 pkg_search_module(GATTLIB REQUIRED gattlib)
@@ -38,9 +46,17 @@ include_directories(${GLIB_INCLUDE_DIRS})
 include_directories(${Mcrypt_INCLUDE_DIRS})
 link_directories(${Mcrypt_LIBS})
 
-add_library(ble_connect_lib include/ble_connect/ble_connect.h src/ble_connect.c)
+add_library(ble_connect include/ble_connect/ble_connect.h src/ble_connect.c)
 
-target_link_libraries(ble_connect_lib ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS})
+target_link_libraries(ble_connect ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS})
 
 add_executable(ble_connect_test tests/ble_test.cpp)
-target_link_libraries(ble_connect_test ble_connect_lib)
+target_link_libraries(ble_connect_test ble_connect)
+
+
+
+add_library(speller ${SRCS} ${FORMS_HPP} ${MOC_HPP} ${RCC} ${HPP})
+target_link_libraries(speller Qt5::Widgets Qt5::Core Qt5::OpenGL Qt5::Gui ${catkin_LIBRARIES})
+
+add_executable(speller_ui src/main.cpp)
+target_link_libraries(speller_ui speller)

qt_pi/include/mapview/mapview.h

@@ -0,0 +1,60 @@
+#ifndef MAPVIEW
+#define MAPVIEW
+
+#include "vertex.h"
+
+#include <QDebug>
+#include <QOpenGLBuffer>
+#include <QOpenGLFunctions_3_1>
+#include <QOpenGLShaderProgram>
+#include <QOpenGLVertexArrayObject>
+#include <QOpenGLWindow>
+#include <QString>
+#include <chrono>
+#include <thread>
+#include <unistd.h>
+
+class QOpenGLShaderProgram;
+
+class MapView : public QOpenGLWindow, protected QOpenGLFunctions_3_1 {
+  Q_OBJECT
+public:
+  explicit MapView(QWidget *parent = 0);
+  ~MapView();
+  void start_blinking();
+  void stop_blinking();
+  volatile int sleep_control = 1;
+
+protected:
+  GLsizei count;
+  GLint startingElements[4] = {0, 4, 8, 12};
+  GLsizei counts[4] = {4, 4, 4, 4};
+  void initializeGL() override;
+  void paintGL() override;
+  void resizeGL(int, int) override;
+  void teardownGL();
+  void printContextInformation();
+signals:
+  void update_GUI();
+
+private:
+  QOpenGLBuffer m_vertex;
+  QOpenGLVertexArrayObject m_object;
+  QOpenGLShaderProgram *m_program;
+  void run();
+  std::thread *t;
+  int blinking;
+  //   QTimer *t1;
+  //   QTimer *t2;
+  //   QTimer *t3;
+  //   QTimer *t4;
+  //   int blinking = 0;
+  //   int state = 0;
+  // private slots:
+  //   void t1Slot();
+  //   void t2Slot();
+  //   void t3Slot();
+  //   void t4Slot();
+};
+
+#endif

qt_pi/include/mapview/vertex.h

@@ -0,0 +1,67 @@
+#ifndef VERTEX_H
+#define VERTEX_H
+
+#include <QVector3D>
+
+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);
+
+  // Accessors / Mutators
+  Q_DECL_CONSTEXPR const QVector3D &position() const;
+  Q_DECL_CONSTEXPR const QVector3D &color() const;
+  void setPosition(const QVector3D &position);
+  void setColor(const QVector3D &color);
+
+  // OpenGL Helpers
+  static const int PositionTupleSize = 3;
+  static const int ColorTupleSize = 3;
+  static Q_DECL_CONSTEXPR int positionOffset();
+  static Q_DECL_CONSTEXPR int colorOffset();
+  static Q_DECL_CONSTEXPR int stride();
+
+private:
+  QVector3D m_position;
+  QVector3D m_color;
+};
+
+/*******************************************************************************
+ * Inline Implementation
+ ******************************************************************************/
+
+// Note: Q_MOVABLE_TYPE means it can be memcpy'd.
+Q_DECLARE_TYPEINFO(Vertex, Q_MOVABLE_TYPE);
+
+// Constructors
+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)
+    : m_position(position), m_color(color) {}
+
+// Accessors / Mutators
+Q_DECL_CONSTEXPR inline const QVector3D &Vertex::position() const {
+  return m_position;
+}
+Q_DECL_CONSTEXPR inline const QVector3D &Vertex::color() const {
+  return m_color;
+}
+void inline Vertex::setPosition(const QVector3D &position) {
+  m_position = position;
+}
+void inline Vertex::setColor(const QVector3D &color) { m_color = color; }
+
+// OpenGL Helpers
+Q_DECL_CONSTEXPR inline int Vertex::positionOffset() {
+  return offsetof(Vertex, m_position);
+}
+Q_DECL_CONSTEXPR inline int Vertex::colorOffset() {
+  return offsetof(Vertex, m_color);
+}
+Q_DECL_CONSTEXPR inline int Vertex::stride() { return sizeof(Vertex); }
+
+#endif // VERTEX_H

qt_pi/include/speller/speller.h

@@ -0,0 +1,18 @@
+#ifndef SPELLER
+#define SPELLER
+
+#include <mapview/mapview.h>
+#include <ros/ros.h>
+
+class Speller {
+
+public:
+  explicit Speller(ros::NodeHandle _nh);
+  ~Speller();
+
+  MapView *window;
+
+private:
+  ros::NodeHandle nh;
+};
+#endif

qt_pi/package.xml

@@ -1,17 +1,19 @@
 <package format="2">
-  <name>qt_pi</name>
+    <name>qt_pi</name>
-  <version>0.0.0</version>
+    <version>0.0.0</version>
-  <description>The qt_pi package</description>
+    <description>The qt_pi package</description>
-  <maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
+    <maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
-  <license>MIT</license>
+    <license>MIT</license>
-  <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>catkin</buildtool_depend>
+    <build_depend>qt_build</build_depend>
+    <build_depend>roscpp</build_depend>
+    <build_depend>roslib</build_depend>
 
-  <exec_depend>rospy</exec_depend>
+    <exec_depend>rospy</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>tf</exec_depend>
+    <exec_depend>tf</exec_depend>
-  <exec_depend>python-serial</exec_depend>
+    <exec_depend>python-serial</exec_depend>
-  <export>
+    <export></export>
-  </export>
 </package>

qt_pi/resources/resources.qrc

@@ -0,0 +1,6 @@
+<RCC>
+    <qresource prefix="/">
+        <file>shaders/simple.frag</file>
+        <file>shaders/simple.vert</file>
+    </qresource>
+</RCC>

qt_pi/resources/shaders/simple.frag

@@ -0,0 +1,8 @@
+#version 330
+in vec4 vColor;
+out vec4 fColor;
+
+void main()
+{
+   fColor = vColor;
+}

qt_pi/resources/shaders/simple.vert

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

qt_pi/src/ble_connect.c

@@ -21,7 +21,7 @@ int data_buffer_length;
 int data_index;
 
 int status = 0;
-int write_to_buffer = 0;
+volatile int write_to_buffer = 0;
 
 pthread_t thread;
 

qt_pi/src/main.cpp

@@ -0,0 +1,23 @@
+#include "speller/speller.h"
+#include <QGuiApplication>
+#include <QSurfaceFormat>
+
+int main(int argc, char *argv[]) {
+  QGuiApplication app(argc, argv);
+  ros::init(argc, argv, "speller");
+  ros::NodeHandle nh;
+  QSurfaceFormat format;
+  format.setSwapInterval(1);
+  format.setRenderableType(QSurfaceFormat::OpenGL);
+  format.setProfile(QSurfaceFormat::CoreProfile);
+  format.setVersion(3, 3);
+
+  // Set the window up
+  Speller spell(nh);
+
+  spell.window->setFormat(format);
+  // spell.window->resize(QSize(800, 600));
+  spell.window->show();
+
+  return app.exec();
+}

qt_pi/src/mapview.cpp

@@ -0,0 +1,210 @@
+#include "mapview/mapview.h"
+
+MapView::MapView(QWidget *parent) {
+  connect(this, SIGNAL(update_GUI()), this, SLOT(update()));
+  blinking = 0;
+  count = 4;
+}
+
+MapView::~MapView() {
+  stop_blinking();
+  makeCurrent();
+  teardownGL();
+}
+
+void MapView::stop_blinking() {
+  if (!blinking)
+    return;
+
+  blinking = 0;
+
+  t->join();
+  free(t);
+}
+
+void MapView::start_blinking() {
+  if (blinking)
+    return;
+  blinking = 1;
+  t = new std::thread(&MapView::run, this);
+  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());
+  }
+
+  // while (blinking) {
+  //   emit(update_GUI());
+  //   printf("WOah\n");
+  //
+  //   count = 4;
+  //   sleep(1);
+  //   emit(update_GUI());
+  //   count = 0;
+  //   sleep(1);
+  // }
+}
+
+static const Vertex sg_vertexes[] = {
+    Vertex(QVector3D(-0.55f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.45f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.45f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.55f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+
+    Vertex(QVector3D(-0.55f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.45f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.45f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(-0.55f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+
+    Vertex(QVector3D(0.55f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.45f, 0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.45f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.55f, 0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+
+    Vertex(QVector3D(0.55f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.45f, -0.55f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.45f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+    Vertex(QVector3D(0.55f, -0.45f, 1.0f), QVector3D(0.75f, 0.75f, 0.75f)),
+
+    // Vertex(QVector3D(-0.75f, 0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(-0.25f, 0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(-0.25f, 0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(-0.75f, 0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    //
+    // Vertex(QVector3D(-0.75f, -0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(-0.25f, -0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(-0.25f, -0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(-0.75f, -0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    //
+    // Vertex(QVector3D(0.75f, 0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(0.25f, 0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(0.25f, 0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(0.75f, 0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    //
+    // Vertex(QVector3D(0.75f, -0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(0.25f, -0.75f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(0.25f, -0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f)),
+    // Vertex(QVector3D(0.75f, -0.25f, 1.0f), QVector3D(0.0f, 0.0f, 0.0f))
+};
+
+void MapView::initializeGL() {
+  initializeOpenGLFunctions();
+  printContextInformation();
+
+  glClearColor(0, 0, 0, 1);
+  {
+    // Create Shader (Do not release until VAO is created)
+    m_program = new QOpenGLShaderProgram();
+    m_program->addShaderFromSourceFile(QOpenGLShader::Vertex,
+                                       ":/shaders/simple.vert");
+    m_program->addShaderFromSourceFile(QOpenGLShader::Fragment,
+                                       ":/shaders/simple.frag");
+    m_program->link();
+    m_program->bind();
+
+    // Create Buffer (Do not release until VAO is created)
+    m_vertex.create();
+    m_vertex.bind();
+    m_vertex.setUsagePattern(QOpenGLBuffer::StaticDraw);
+    m_vertex.allocate(sg_vertexes, sizeof(sg_vertexes));
+
+    // Create Vertex Array Object
+    m_object.create();
+    m_object.bind();
+    m_program->enableAttributeArray(0);
+    m_program->enableAttributeArray(1);
+    m_program->setAttributeBuffer(0, GL_FLOAT, Vertex::positionOffset(),
+                                  Vertex::PositionTupleSize, Vertex::stride());
+    m_program->setAttributeBuffer(1, GL_FLOAT, Vertex::colorOffset(),
+                                  Vertex::ColorTupleSize, Vertex::stride());
+
+    // Release (unbind) all
+    m_object.release();
+    m_vertex.release();
+    m_program->release();
+  }
+}
+
+void MapView::paintGL() {
+  glClear(GL_COLOR_BUFFER_BIT);
+  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) {
+  glViewport(0, 0, w, h);
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
+}
+
+void MapView::printContextInformation() {
+  QString glType;
+  QString glVersion;
+  QString glProfile;
+
+  // Get Version Information
+  glType = (context()->isOpenGLES()) ? "OpenGL ES" : "OpenGL";
+  glVersion = reinterpret_cast<const char *>(glGetString(GL_VERSION));
+
+  // Get Profile Information
+#define CASE(c)                                                                \
+  case QSurfaceFormat::c:                                                      \
+    glProfile = #c;                                                            \
+    break
+  switch (format().profile()) {
+    CASE(NoProfile);
+    CASE(CoreProfile);
+    CASE(CompatibilityProfile);
+  }
+#undef CASE
+
+  // qPrintable() will print our QString w/o quotes around it.
+  qDebug() << qPrintable(glType) << qPrintable(glVersion) << "("
+           << qPrintable(glProfile) << ")";
+}
+
+void MapView::teardownGL() {
+  m_object.destroy();
+  m_vertex.destroy();
+  delete m_program;
+}

qt_pi/src/speller.cpp

@@ -0,0 +1,5 @@
+#include "speller/speller.h"
+
+Speller::Speller(ros::NodeHandle _nh) : nh(_nh) { window = new MapView(); }
+
+Speller::~Speller() {}

qt_pi/tests/ble_test.cpp

@@ -8,9 +8,9 @@ int main() {
   insight_start_notif();
 
   struct insight_data *buf =
-      (struct insight_data *)malloc(sizeof(struct insight_data) * 10);
+      (struct insight_data *)malloc(sizeof(struct insight_data) * 1000);
 
-  int len = 10;
+  int len = 1000;
 
   insight_write_to_buffer(len, buf);
 
@@ -22,7 +22,9 @@ int main() {
   printf("Stopping notif %d \n", insight_stop_notif());
 
   for (int i = 0; i < len; i++) {
-    printf("%f\n", buf[i].uV[0]);
+    for (int j = 0; j < 5; j++)
+      printf("%f ", buf[i].uV[j]);
+    printf("\n");
   }
 
   printf("Destroy called %d\n", insight_destroy());