diff --git a/qt_pi/include/mapview/mapview.h b/qt_pi/include/mapview/mapview.h index 3ee4752..c5ccaec 100644 --- a/qt_pi/include/mapview/mapview.h +++ b/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 diff --git a/qt_pi/include/speller/speller.h b/qt_pi/include/speller/speller.h index 906f936..bad4d9a 100644 --- a/qt_pi/include/speller/speller.h +++ b/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 @@ -30,8 +29,14 @@ private: char **argv; void run() override; - enum Map_Type { SQUARE, HORIZONTAL, VERTICAL }; - int in_view; + enum Map_Shape { SQUARE, HORIZONTAL, VERTICAL }; + 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 diff --git a/qt_pi/src/mapview.cpp b/qt_pi/src/mapview.cpp index 90fdb53..ce49d7d 100644 --- a/qt_pi/src/mapview.cpp +++ b/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; - map_redraw = -1; + count = 0; + 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; - count++; + if (((sq_state >> i) & 0b1)) + 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(); - { - m_object.bind(); - glMultiDrawArrays(GL_TRIANGLE_FAN, startingElements, counts, count); - m_object.release(); - } - m_program->release(); + glClear(GL_COLOR_BUFFER_BIT); 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) { - - 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(); +void MapView::load_texture(int len, Vertex *ver, QImage *map_img) { + 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.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(); - + map_vertex.write(0, vertices, vertices_len); 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)); } diff --git a/qt_pi/src/speller.cpp b/qt_pi/src/speller.cpp index 858475f..c72630b 100644 --- a/qt_pi/src/speller.cpp +++ b/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, - SLOT(load_texture(int, Vertex *, QImage))); + connect(this, SIGNAL(load_texture(int, Vertex *, QImage *)), window, + 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() { - - 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; - } +void Speller::classify(int len, struct insight_data *buf) { + sleep(2); + if ((me.x() * me.y()) < 0) { + if (me.x() < 0) + classifier_result = TOP_LEFT; + else + classifier_result = BOTTOM_RIGHT; + } else { + if (me.x() < 0) + classifier_result = BOTTOM_LEFT; + else + classifier_result = TOP_RIGHT; } +} - 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, - QImage::Format_RGB888); +__inline__ void Speller::load_map() { + 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); - sleep(10); - while ((!ret) && (insight_buffer_status() == 1)) - sleep(0); + // int ret = insight_write_to_buffer(buf_len, buf); + // 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()); +}