diff --git a/gazebo_sim/launch/gazebo_sim.launch b/gazebo_sim/launch/gazebo_sim.launch index cb606b8..64b5ae2 100644 --- a/gazebo_sim/launch/gazebo_sim.launch +++ b/gazebo_sim/launch/gazebo_sim.launch @@ -5,5 +5,5 @@ - - \ No newline at end of file + + diff --git a/qt_pi/CMakeLists.txt b/qt_pi/CMakeLists.txt index 89b083e..d858d67 100644 --- a/qt_pi/CMakeLists.txt +++ b/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) diff --git a/qt_pi/include/ble_connect/ble_connect.h b/qt_pi/include/ble_connect/ble_connect.h index c5cccbc..d200262 100644 --- a/qt_pi/include/ble_connect/ble_connect.h +++ b/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(); diff --git a/qt_pi/include/mapview/mapview.h b/qt_pi/include/mapview/mapview.h index 87821d7..3ee4752 100644 --- a/qt_pi/include/mapview/mapview.h +++ b/qt_pi/include/mapview/mapview.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -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; - // QTimer *t3; - // QTimer *t4; - // int blinking = 0; - // int state = 0; - // private slots: - // void t1Slot(); - // void t2Slot(); - // void t3Slot(); - // void t4Slot(); + +signals: + void update_GUI(); + void keyPress(QKeyEvent *key); + void map_loaded(); +private slots: + void load_texture(int, Vertex *texture, QImage map_img); }; #endif diff --git a/qt_pi/include/mapview/vertex.h b/qt_pi/include/mapview/vertex.h index 9530a26..7b5f2dd 100644 --- a/qt_pi/include/mapview/vertex.h +++ b/qt_pi/include/mapview/vertex.h @@ -1,31 +1,40 @@ #ifndef VERTEX_H #define VERTEX_H +#include #include +#include 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 diff --git a/qt_pi/include/speller/speller.h b/qt_pi/include/speller/speller.h index 5d90381..906f936 100644 --- a/qt_pi/include/speller/speller.h +++ b/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 @@ -10,6 +11,9 @@ #include #include #include +#include +#include +#include #include 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); - -private: - volatile int spelling; + void map_loaded(); +signals: + void load_texture(int, Vertex *, QImage); }; #endif diff --git a/qt_pi/resources/playground.pgm b/qt_pi/resources/playground.pgm new file mode 100644 index 0000000..f4af84d Binary files /dev/null and b/qt_pi/resources/playground.pgm differ diff --git a/qt_pi/resources/resources.qrc b/qt_pi/resources/resources.qrc index 0248cfc..4a04629 100644 --- a/qt_pi/resources/resources.qrc +++ b/qt_pi/resources/resources.qrc @@ -1,6 +1,9 @@ - shaders/simple.frag - shaders/simple.vert + shaders/blink.frag + shaders/blink.vert + shaders/map.vert + shaders/map.frag + playground.pgm diff --git a/qt_pi/resources/shaders/simple.frag b/qt_pi/resources/shaders/blink.frag similarity index 100% rename from qt_pi/resources/shaders/simple.frag rename to qt_pi/resources/shaders/blink.frag diff --git a/qt_pi/resources/shaders/simple.vert b/qt_pi/resources/shaders/blink.vert similarity index 65% rename from qt_pi/resources/shaders/simple.vert rename to qt_pi/resources/shaders/blink.vert index 10ded2e..e4f8480 100644 --- a/qt_pi/resources/shaders/simple.vert +++ b/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; } diff --git a/qt_pi/resources/shaders/map.frag b/qt_pi/resources/shaders/map.frag new file mode 100644 index 0000000..7d20a87 --- /dev/null +++ b/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); +} diff --git a/qt_pi/resources/shaders/map.vert b/qt_pi/resources/shaders/map.vert new file mode 100644 index 0000000..49944eb --- /dev/null +++ b/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; +} diff --git a/qt_pi/src/ble_connect.c b/qt_pi/src/ble_connect.c index 7e9c683..a2eccf0 100644 --- a/qt_pi/src/ble_connect.c +++ b/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() { diff --git a/qt_pi/src/main.cpp b/qt_pi/src/main.cpp index 69e200a..89126f2 100644 --- a/qt_pi/src/main.cpp +++ b/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(); } diff --git a/qt_pi/src/mapview.cpp b/qt_pi/src/mapview.cpp index 1937304..90fdb53 100644 --- a/qt_pi/src/mapview.cpp +++ b/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::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::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.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, 0.0f), + QVector4D(0.75f, 0.75f, 0.75f, 1.0f)), + Vertex(QVector3D(-0.45f, 0.55f, 0.0f), + 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.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, 0.0f), + QVector4D(0.75f, 0.75f, 0.75f, 1.0f)), + Vertex(QVector3D(-0.45f, -0.55f, 0.0f), + 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.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, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)), + Vertex(QVector3D(0.45f, 0.55f, 0.0f), 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.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, 0.0f), + QVector4D(0.75f, 0.75f, 0.75f, 1.0f)), + Vertex(QVector3D(0.45f, -0.55f, 0.0f), + 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)); } diff --git a/qt_pi/src/speller.cpp b/qt_pi/src/speller.cpp index 40843a3..858475f 100644 --- a/qt_pi/src/speller.cpp +++ b/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 *))); - - const char *a = "EF:BD:39:4E:08:49"; - const char *b = "81072f41-9f3d-11e3-a9dc-0002a5d5c51b"; - insight_init(a, b); + connect(this, SIGNAL(load_texture(int, Vertex *, QImage)), window, + SLOT(load_texture(int, Vertex *, QImage))); + connect(window, SIGNAL(map_loaded()), this, SLOT(map_loaded())); } Speller::~Speller() { + insight_stop_notif(); insight_destroy(); - this->terminate(); + ros::shutdown(); + window->~MapView(); + this->wait(); } void Speller::run() { - const std::string node_name = "speller"; - - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::Publisher pub = - nh.advertise("/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; - + // init_ble(); + init_ros(); + load_map(); + ros::Rate loop(10); + ros::Rate loop2(10); while (ros::ok()) { - while (spelling) { - int len = 1000; - struct insight_data *buf = - (struct insight_data *)malloc(sizeof(struct insight_data) * len); - - window->start_blinking(); - insight_write_to_buffer(len, buf); - - while (insight_buffer_status() == 1) - sleep(0); - - window->stop_blinking(); - + loop2.sleep(); + while (spelling && ros::ok()) { + get_ble_data(); + loop.sleep(); // 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("/move_base_simple/goal", 1); + map_client = nh->serviceClient("/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 diff --git a/qt_pi/tests/map_load_test.cpp b/qt_pi/tests/map_load_test.cpp new file mode 100644 index 0000000..af47e1d --- /dev/null +++ b/qt_pi/tests/map_load_test.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include +#include + +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("/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(); +}