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

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/globalDefinitions.h

 #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

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h

@@ -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
 };
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/marker.h

@@ -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;
 };
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/rosNodeThread.h

@@ -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

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFlyZone.cpp

@@ -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);
 }
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp

@@ -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);

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/marker.cpp

@@ -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;
     }
 }

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsScene.cpp

@@ -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);

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/rosNodeThread.cpp

 #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()

pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp

@@ -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