mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-03 21:54:12 +05:30
Select and zoom into map done
This commit is contained in:
yikestone
parent
ac52430f2a
commit
e4629f0e6f
@ -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
|
||||
|
||||
@ -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 };
|
||||
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
|
||||
|
||||
@ -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)); }
|
||||
|
||||
@ -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;
|
||||
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 {
|
||||
map_tex[3 * i + 0] = 255;
|
||||
map_tex[3 * i + 1] = 255;
|
||||
map_tex[3 * i + 2] = 255;
|
||||
if (me.x() < 0)
|
||||
classifier_result = BOTTOM_LEFT;
|
||||
else
|
||||
classifier_result = TOP_RIGHT;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
map_loading_done = 0;
|
||||
__inline__ void Speller::load_map() {
|
||||
float max = std::max(area_displayed->width(), area_displayed->height());
|
||||
|
||||
QImage tmp(map_tex, map.info.width, map.info.height, 3 * map.info.width,
|
||||
QImage::Format_RGB888);
|
||||
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());
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user