Commit ae577cba authored by roangel's avatar roangel
Browse files

Fixed problem Moving markers. Still need to find out a way of moving the...

Fixed problem Moving markers. Still need to find out a way of moving the anti-zooming highlight circle
parent 027eed6c
#ifndef GLOBAL_DEFINITIONS_H
#define GLOBAL_DEFINITIONS_H
#define TO_METERS 100
#define TO_CENTIMETERS 1
#define TO_MILIMETERS 0.1
#define FROM_UNITS_TO_METERS 0.01
#define FROM_UNITS_TO_CENTIMETERS 1
#define FROM_UNITS_TO_MILIMETERS 10
#define FROM_METERS_TO_UNITS 100
#define FROM_CENTIMETERS_TO_UNITS 1
#define FROM_MILIMETERS_TO_UNITS 0.1
#endif
......@@ -97,6 +97,8 @@ private slots:
void on_checkBox_vicon_highlight_markers_toggled(bool checked);
void setPosMarker(double x, double y);
private:
Ui::MainGUIWindow *ui;
......@@ -106,7 +108,7 @@ private:
Marker* marker;
#ifdef CATKIN_MAKE
rosNodeThread _rosNodeThread;
rosNodeThread* _rosNodeThread;
#endif
};
......
......@@ -5,14 +5,13 @@
#include <QGraphicsEllipseItem>
#define MARKER_DIAMETER 20 * TO_MILIMETERS
#define MARKER_DIAMETER 20 * FROM_MILIMETERS_TO_UNITS
#define HIGHLIGHT_DIAMETER 20
#define HIGHLIGHT_WIDTH 5
class Marker : public QGraphicsEllipseItem
{
public:
explicit Marker(qreal x, qreal y, QGraphicsItem *parent = 0);
~Marker();
......@@ -22,16 +21,23 @@ public:
void clearHighlighted(void);
bool getHighlighted(void);
void setPosMarker(QPointF new_p);
private:
// properties of marker itself
qreal _diameter;
qreal _x;
qreal _x; // coordinates of top-left corner of marker
qreal _y;
qreal _center_x; // coordinates of center of marker
qreal _center_y;
bool _highlighted;
QGraphicsEllipseItem* _highlight_circle;
qreal _highlight_diameter;
qreal _x_highlight;
qreal _x_highlight; // coordinates of highlighting circle's top-left corner
qreal _y_highlight;
};
......
......@@ -18,16 +18,22 @@ using namespace d_fall_pps;
class rosNodeThread : public QObject {
Q_OBJECT
public:
rosNodeThread(int argc, char **pArgv, const char * topic);
explicit rosNodeThread(int argc, char **pArgv, const char * topic, QObject *parent = 0);
virtual ~rosNodeThread();
bool init();
void messageCallback(const ViconData& data);
Q_SLOT void run();
// Q_SIGNAL void newPose(double,double,double);
signals:
// void newViconData(double, double, double, double, double, double);
void newViconData(double, double);
public slots:
void run();
private:
int m_Init_argc;
char** m_pInit_argv;
......@@ -35,7 +41,9 @@ private:
QThread * m_pThread;
ros::Subscriber test;
ViconData m_vicon_data;
ros::Subscriber m_vicon_subscriber;
// ros::Publisher sim_velocity;
};
#endif
......
......@@ -24,8 +24,8 @@ void crazyFlyZone::setLabel(QString string)
void crazyFlyZone::setLabelPosition()
{
qreal x_offset = 0.1 * TO_METERS;
qreal y_offset = 0.05 * TO_METERS;
qreal x_offset = 0.1 * FROM_METERS_TO_UNITS;
qreal y_offset = 0.05 * FROM_METERS_TO_UNITS;
label->setPos(this->rect().topLeft().x() + x_offset,this->rect().topLeft().y() + y_offset);
}
......
......@@ -20,9 +20,10 @@ using namespace d_fall_pps;
#ifdef CATKIN_MAKE
MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainGUIWindow),
_rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData")
ui(new Ui::MainGUIWindow)//,
// _rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData")
{
_rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData");
ui->setupUi(this);
_init();
......@@ -59,19 +60,15 @@ void MainGUIWindow::set_tabs(int n)
void MainGUIWindow::_init()
{
// #ifdef CATKIN_MAKE
// ViconSubscriber = new ros::Subscriber(_pNodeHandle->subscribe("/ViconDataPublisher/ViconData", 1, viconCallback));
// ros::spin();
// ROS_INFO("successfully subscribed to ViconData from GUI");
// qDebug("successfully subscribed to ViconData from GUI");
// #endif
ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
scene = new myGraphicsScene(ui->frame_drawing);
scene->setSceneRect(-100 * TO_METERS, -100 * TO_METERS, 200 * TO_METERS, 200 * TO_METERS);
scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
// Marker* marker = new Marker(0, 0);
marker = new Marker(1 * FROM_METERS_TO_UNITS, 1 * FROM_METERS_TO_UNITS);
// marker->setPos(0,0);
setPosMarker(0, 0);
// scene->addItem(marker);
ui->graphicsView->setScene(scene);
......@@ -85,10 +82,15 @@ void MainGUIWindow::_init()
ui->checkBox_vicon_highlight_markers->setEnabled(false);
#ifdef CATKIN_MAKE
_rosNodeThread.init();
_rosNodeThread->init();
QObject::connect(_rosNodeThread, SIGNAL(newViconData(double, double)), this, SLOT(setPosMarker(double, double)));
#endif
}
void MainGUIWindow::setPosMarker(double x, double y)
{
marker->setPosMarker(scene->mapFromWorldToScene(QPointF(FROM_METERS_TO_UNITS * x, FROM_METERS_TO_UNITS * y)));
}
void MainGUIWindow::on_removeTable_clicked()
......@@ -243,15 +245,16 @@ void MainGUIWindow::on_checkBox_vicon_markers_toggled(bool checked)
// This is temporal, just to see effect. In the end the marker will be created with data from vicon
if(checked)
{
marker = new Marker(0, 0);
// marker = new Marker(0, 0);
scene->addItem(marker);
ui->checkBox_vicon_highlight_markers->setCheckable(true);
ui->checkBox_vicon_highlight_markers->setEnabled(true);
}
else
{
marker->setParentItem(NULL);
delete marker;
scene->removeItem(marker);
// marker->setParentItem(NULL);
// delete marker;
ui->checkBox_vicon_highlight_markers->setChecked(false);
ui->checkBox_vicon_highlight_markers->setCheckable(false);
ui->checkBox_vicon_highlight_markers->setEnabled(false);
......
......@@ -9,28 +9,54 @@ Marker::Marker(qreal x, qreal y, QGraphicsItem * parent)
{
_highlighted = false;
_highlight_diameter = HIGHLIGHT_DIAMETER;
_x_highlight = x - _highlight_diameter/2;
_y_highlight = y -_highlight_diameter/2;
// save original x and y
_center_x = x;
_center_y = y;
_diameter = MARKER_DIAMETER; // x and y are top left coordinates
_x = x - _diameter/2;
_y = y - _diameter/2;
_x = _center_x - _diameter/2;
_y = _center_y - _diameter/2;
this->setRect(QRectF(_x, _y, _diameter, _diameter));
this->setPen(Qt::NoPen);
this->setBrush(QColor(255, 0, 0));
this->setZValue(10); // max z value, should always be seen
}
void Marker::setPosMarker(QPointF new_p)
{
prepareGeometryChange();
_center_x = new_p.x(); // update center coordinates
_center_y = new_p.y();
_x = _center_x - _diameter/2;
_y = _center_y - _diameter/2;
// this->setPos(_x, _y); //update top-left corner coordinates
this->setRect(QRectF(_x, _y, _diameter, _diameter));
if(_highlighted)
{
_x_highlight = _center_x - _highlight_diameter/2; // update top-left corner coordinates of highlighing circle
_y_highlight = _center_y -_highlight_diameter/2;
// _highlight_circle->setPos(_x_highlight, _y_highlight);
_highlight_circle->setRect(QRectF(_x_highlight, _y_highlight, _highlight_diameter, _highlight_diameter));
}
}
void Marker::setHighlighted(void)
{
if(!_highlighted)
{
_x_highlight = _center_x - _highlight_diameter/2;
_y_highlight = _center_y -_highlight_diameter/2;
prepareGeometryChange();
_highlight_circle = new QGraphicsEllipseItem();
_highlight_circle->setRect(QRectF(_x_highlight, _y_highlight, _highlight_diameter, _highlight_diameter));
_highlight_circle->setPen(QPen(QBrush(Qt::black), HIGHLIGHT_WIDTH));
_highlight_circle->setParentItem(this);
_highlight_circle->setFlag(QGraphicsItem::ItemIgnoresTransformations);
// _highlight_circle->setFlag(QGraphicsItem::ItemIgnoresTransformations);
_highlighted = true;
}
}
......
......@@ -333,7 +333,7 @@ void myGraphicsScene::drawBackground(QPainter *painter, const QRectF &rect)
if(grid_enable)
{
const int gridSize = 1 * TO_METERS;
const int gridSize = 1 * FROM_METERS_TO_UNITS;
qreal left = int(rect.left()) - (int(rect.left()) % gridSize);
qreal top = int(rect.top()) - (int(rect.top()) % gridSize);
......
#include "rosNodeThread.h"
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * topic)
: m_Init_argc(argc),
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * topic, QObject* parent)
: QObject(parent),
m_Init_argc(argc),
m_pInit_argv(pArgv),
m_topic(topic)
......@@ -25,8 +26,8 @@ bool rosNodeThread::init()
m_pThread = new QThread();
this->moveToThread(m_pThread); // QObject method
connect(m_pThread, &QThread::started, this, &rosNodeThread::run);
ros::init(m_Init_argc, m_pInit_argv, "my_GUI"); // GUI is the name of this node
connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
ros::init(m_Init_argc, m_pInit_argv, "my_GUI"); // my_GUI is the name of this node
if (!ros::master::check())
return false; // do not start without ros.
......@@ -36,7 +37,7 @@ bool rosNodeThread::init()
ros::NodeHandle nh("~");
// sim_velocity = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
test = nh.subscribe(m_topic, 10, &rosNodeThread::messageCallback, this);
m_vicon_subscriber = nh.subscribe(m_topic, 100, &rosNodeThread::messageCallback, this);
m_pThread->start();
return true;
......@@ -47,9 +48,16 @@ void rosNodeThread::messageCallback(const ViconData& data) // When a message arr
QMutex * pMutex = new QMutex();
pMutex->lock();
ROS_INFO_STREAM("ViconData: " << data.x << ", " << data.y << ", " << data.z);
m_vicon_data.x = data.x;
m_vicon_data.y = data.y;
m_vicon_data.z = data.z;
m_vicon_data.yaw = data.yaw;
m_vicon_data.pitch = data.pitch;
m_vicon_data.roll = data.roll;
pMutex->unlock();
delete pMutex;
// Q_EMIT newPose(m_xPos, m_yPos, m_aPos);
// Q_EMIT newViconData(m_vicon_data.x, m_vicon_data.y, m_vicon_data.z, m_vicon_data.yaw, m_vicon_data.pitch, m_vicon_data.roll);
emit newViconData(m_vicon_data.x, m_vicon_data.y);
}
void rosNodeThread::run()
......
......@@ -46,17 +46,19 @@ int main(int argc, char* argv[]) {
//publish something random if no viconData is available for testing
ViconData viconData;
int i = 1;
float f = 0;
int i = 0;
while(ros::ok())
{
if(i % 1000 == 0)
{
ROS_INFO("iteration #%d",i);
}
viconData.x = i;
viconData.x = f;
viconDataPublisher.publish(viconData);
ros::Duration(0.001).sleep();
++i;
ros::Duration(0.1).sleep();
f += 0.01;
i++;
}
//the code will not go further than here if testing without real ViconData
......@@ -105,7 +107,7 @@ int main(int argc, char* argv[]) {
// Output_GetUnlabeledMarkerCount GetUnlabeledMarkerCount() const;
// Get a frame
while (client.GetFrame().Result != Result::Success) {
// Sleep a little so that we don't lumber the CPU with a busy poll
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment