Commit ba7fb0d1 authored by roangel's avatar roangel
Browse files

Merge branch 'from_vicon_data_to_plotting_cfs' into pps_project

parents c4c55f67 8db7c445
......@@ -28,9 +28,7 @@ SOURCES += \
src/myGraphicsRectItem.cpp \
src/myGraphicsScene.cpp \
src/myGraphicsView.cpp \
src/tablePiece.cpp \
src/marker.cpp \
src/crazyFly.cpp
src/tablePiece.cpp
HEADERS += \
include/cornergrabber.h \
......
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.0.2, 2017-05-11T18:09:24. -->
<!-- Written by QtCreator 4.0.2, 2017-05-12T16:06:37. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
......
......@@ -6,6 +6,13 @@
#include <QGraphicsSvgItem>
#include <QSvgRenderer>
#ifdef CATKIN_MAKE
#include "d_fall_pps/CrazyflieData.h"
#endif
#ifdef CATKIN_MAKE
using namespace d_fall_pps;
#endif
#define DRONE_HEIGHT 100 * FROM_MILIMETERS_TO_UNITS
#define DRONE_WIDTH 100 * FROM_MILIMETERS_TO_UNITS
......@@ -13,14 +20,31 @@
class crazyFly : public QGraphicsSvgItem
{
public:
explicit crazyFly(QPointF position, QGraphicsItem * parent = 0);
explicit crazyFly(const CrazyflieData* p_crazyfly_msg, QGraphicsItem * parent = 0);
~crazyFly();
QRectF boundingRect() const;
void paint(QPainter * painter,
const QStyleOptionGraphicsItem * option,
QWidget * widget);
void updateCF(const CrazyflieData* p_crazyfly_msg);
std::string getName();
private:
// info to fill by message
std::string m_name;
qreal m_x;
qreal m_y;
qreal m_z;
qreal m_roll;
qreal m_pitch;
qreal m_yaw;
// info for plotting CF
qreal m_width;
qreal m_height;
};
......
......@@ -9,4 +9,9 @@
#define FROM_CENTIMETERS_TO_UNITS 1
#define FROM_MILIMETERS_TO_UNITS 0.1
#define PI 3.1415926
#define FROM_RADIANS_TO_DEGREES 180.0/PI
#define FROM_DEGREES_TO_RADIANS PI/180.0
#endif
......@@ -10,16 +10,14 @@
#ifdef CATKIN_MAKE
#include "rosNodeThread.h"
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/UnlabeledMarkersArray.h"
#include "marker.h"
#include "crazyFly.h"
#endif
#include "ui_mainguiwindow.h"
#include "myGraphicsScene.h"
#include "globalDefinitions.h"
#include "marker.h" // temporal, just to check
#include "crazyFly.h"
namespace Ui {
class MainGUIWindow;
......@@ -100,7 +98,7 @@ private slots:
void on_checkBox_vicon_highlight_markers_toggled(bool checked);
#ifdef CATKIN_MAKE
void setPosMarkers(const ptrToMessage& p_msg);
void updateNewViconData(const ptrToMessage& p_msg);
#endif
private:
......@@ -108,12 +106,10 @@ private:
myGraphicsScene* scene;
void _init();
std::vector<Marker*> markers_vector;
crazyFly* crazyFly_1;
#ifdef CATKIN_MAKE
rosNodeThread* _rosNodeThread;
std::vector<Marker*> markers_vector;
std::vector<crazyFly*> crazyflies_vector;
#endif
};
......
......@@ -5,6 +5,15 @@
#include <QGraphicsEllipseItem>
#ifdef CATKIN_MAKE
#include "d_fall_pps/UnlabeledMarker.h"
#endif
#ifdef CATKIN_MAKE
using namespace d_fall_pps;
#endif
#define MARKER_DIAMETER 20 * FROM_MILIMETERS_TO_UNITS
#define HIGHLIGHT_DIAMETER 20
......@@ -14,7 +23,7 @@
class Marker : public QGraphicsEllipseItem
{
public:
explicit Marker(QPointF p, QGraphicsItem *parent = 0);
explicit Marker(const UnlabeledMarker* p_marker_msg, QGraphicsItem *parent = 0);
~Marker();
void setHighlighted(void);
......@@ -23,13 +32,17 @@ public:
bool getHighlighted(void);
void setPosMarker(QPointF new_p);
void updateMarker(const UnlabeledMarker* p_marker_msg);
private:
// info to fill by message
qreal m_x;
qreal m_y;
qreal m_z;
// properties of marker itself
qreal _diameter;
qreal _x; // coordinates of top-left corner of marker
qreal _y;
qreal _center_x; // coordinates of center of marker
qreal _center_y;
......
......@@ -4,18 +4,41 @@
#include <QBrush>
crazyFly::crazyFly(QPointF position, QGraphicsItem * parent)
crazyFly::crazyFly(const CrazyflieData* p_crazyfly_msg, QGraphicsItem * parent)
: QGraphicsSvgItem(":/images/drone_fixed.svg")
{
updateCF(p_crazyfly_msg);
m_width = DRONE_WIDTH;
m_height = DRONE_HEIGHT;
this->setPos(position);
}
crazyFly::~crazyFly()
{
}
std::string crazyFly::getName()
{
return m_name;
}
void crazyFly::updateCF(const CrazyflieData* p_crazyfly_msg)
{
m_name = p_crazyfly_msg->crazyflieName;
m_x = p_crazyfly_msg->x;
m_y = p_crazyfly_msg->y;
m_z = p_crazyfly_msg->z;
m_yaw = p_crazyfly_msg->yaw;
m_pitch = p_crazyfly_msg->pitch;
m_roll = p_crazyfly_msg->roll;
this->setPos(m_x * FROM_MILIMETERS_TO_UNITS, -m_y * FROM_MILIMETERS_TO_UNITS); // - y because of coordinates
this->setRotation(- m_yaw * FROM_RADIANS_TO_DEGREES); //negative beacause anti-clock wise should be positive
}
QRectF crazyFly::boundingRect() const
{
// return QRectF(-original_width/2, -original_height/2, original_width, original_height);
......
......@@ -11,6 +11,10 @@
#include <QMetaType>
#include <QDir>
#ifdef CATKIN_MAKE
#include "d_fall_pps/UnlabeledMarker.h"
#endif
#include <string>
#define N_MAX_CRAZYFLIES 20 // protection number
......@@ -69,8 +73,6 @@ void MainGUIWindow::_init()
scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
ui->graphicsView->setScene(scene);
crazyFly_1 = new crazyFly(QPointF(50,50));
scene->addItem(crazyFly_1);
QObject::connect(ui->tabWidget, SIGNAL(tabCloseRequested(int)), scene, SLOT(removeCrazyFlyZone(int)));
QObject::connect(scene, SIGNAL(numCrazyFlyZonesChanged(int)), this, SLOT(set_tabs(int)));
......@@ -84,14 +86,16 @@ void MainGUIWindow::_init()
#ifdef CATKIN_MAKE
_rosNodeThread->init();
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(setPosMarkers(const ptrToMessage&)));
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
#endif
}
#ifdef CATKIN_MAKE
void MainGUIWindow::setPosMarkers(const ptrToMessage& p_msg) //connected to newViconData, from node
void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
// update Markers
if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared, received stuff is smaller than what we have
{
for(int i = p_msg->markers.size(); i < markers_vector.size(); i++)
......@@ -110,10 +114,10 @@ void MainGUIWindow::setPosMarkers(const ptrToMessage& p_msg) //connected to newV
{
ROS_INFO_STREAM("element index: " << i << " added");
QPointF p(p_msg->markers[i].x * FROM_MILIMETERS_TO_UNITS, p_msg->markers[i].y * FROM_MILIMETERS_TO_UNITS);
Marker* tmp_p_marker = new Marker(scene->mapFromWorldToScene(p));
Marker* tmp_p_marker = new Marker(&(p_msg->markers[i]));
markers_vector.push_back(tmp_p_marker); // what happens with the new indexes? check if this is correct
if(ui->checkBox_vicon_markers->checkState() == Qt::Checked) //only if markers checkbox info is checked..
if(ui->checkBox_vicon_markers->checkState() == Qt::Checked) //only if markers checkbox info is checked..
{
scene->addItem(markers_vector[i]);
if(ui->checkBox_vicon_highlight_markers->checkState() == Qt::Checked)
......@@ -125,7 +129,56 @@ void MainGUIWindow::setPosMarkers(const ptrToMessage& p_msg) //connected to newV
else
{
ROS_INFO_STREAM("element index: " << i << " moved, already existed");
markers_vector[i]->setPosMarker(scene->mapFromWorldToScene(QPointF(p_msg->markers[i].x * FROM_MILIMETERS_TO_UNITS, p_msg->markers[i].y * FROM_MILIMETERS_TO_UNITS)));
markers_vector[i]->updateMarker(&(p_msg->markers[i]));
}
}
// update Crazyflies
// also: what happens if we dont go through one of the names? we need to remove that crazyfly
int crazyfly_vector_size_before = crazyflies_vector.size(); //initial size of vector
// in this loop, add new ones and update old ones
for(int i = 0; i < p_msg->crazyflies.size(); i++)
{
bool name_found = false; // for each iteration, name_found starts in false
int index_name_found;
for(int j = 0; j < crazyfly_vector_size_before; j++)
{
if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
{
name_found = true; // name found. This can only happen once per i-iteration, names are unique
index_name_found = j; // index in already existing vector, to update it later (really needed?)
}
}
if(name_found)
{
crazyflies_vector[index_name_found]->updateCF(&(p_msg->crazyflies[i]));
}
else
{
crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]));
scene->addItem(tmp_p_crazyfly);
crazyflies_vector.push_back(tmp_p_crazyfly);
}
}
// in this loop, clean the ones that are not present anymore
int crazyfly_vector_size_after = crazyflies_vector.size();
for(int j = 0; j < crazyfly_vector_size_after; j++)
{
bool name_found = false;
for(int i = 0; i < p_msg->crazyflies.size(); i++)
{
if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
{
name_found = true;
}
}
if(!name_found)
{
scene->removeItem(crazyflies_vector[j]);
crazyflies_vector.erase(crazyflies_vector.begin() + j);
}
}
}
......@@ -281,7 +334,7 @@ void MainGUIWindow::on_pushButton_fitAll_clicked()
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
#ifdef CATKIN_MAKE
if(checked)
{
for(int i = 0; i < markers_vector.size(); i++)
......@@ -301,10 +354,12 @@ void MainGUIWindow::on_checkBox_vicon_markers_toggled(bool checked)
ui->checkBox_vicon_highlight_markers->setCheckable(false);
ui->checkBox_vicon_highlight_markers->setEnabled(false);
}
#endif
}
void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
{
#ifdef CATKIN_MAKE
if(checked)
{
for(int i = 0; i < markers_vector.size(); i++)
......@@ -319,4 +374,5 @@ void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
markers_vector[i]->clearHighlighted();
}
}
#endif
}
......@@ -4,35 +4,32 @@
#include <QBrush>
Marker::Marker(QPointF p, QGraphicsItem * parent)
Marker::Marker(const UnlabeledMarker* p_marker_msg, QGraphicsItem * parent)
: QGraphicsEllipseItem(-MARKER_DIAMETER/2, - MARKER_DIAMETER/2, MARKER_DIAMETER, MARKER_DIAMETER, parent)
{
updateMarker(p_marker_msg);
_highlighted = false;
_highlight_diameter = HIGHLIGHT_DIAMETER;
// save original x and y
_center_x = p.x();
_center_y = p.y();
_diameter = MARKER_DIAMETER; // x and y are top left coordinates
this->setPos(_center_x, _center_y); //where it is now, it is the center
_x_highlight = _center_x - _highlight_diameter/2; // update top-left corner coordinates of highlighing circle
_y_highlight = _center_y - _highlight_diameter/2;
_x_highlight = m_x - _highlight_diameter/2; // update top-left corner coordinates of highlighing circle
_y_highlight = m_y - _highlight_diameter/2;
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)
void Marker::updateMarker(const UnlabeledMarker* p_marker_msg)
{
prepareGeometryChange();
this->setPos(_center_x, _center_y);
_center_x = new_p.x(); // update center coordinates
_center_y = new_p.y();
m_x = p_marker_msg->x;
m_y = p_marker_msg->y;
m_z = p_marker_msg->z;
this->setPos(m_x * FROM_MILIMETERS_TO_UNITS, -m_y * FROM_MILIMETERS_TO_UNITS); // - y because of coordinates
}
void Marker::setHighlighted(void)
{
if(!_highlighted)
......
......@@ -20,7 +20,7 @@
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
//#define TESTING_FAKE_DATA
#define TESTING_FAKE_DATA
// notice that unit here are in milimeters
using namespace ViconDataStreamSDK::CPP;
......@@ -79,7 +79,14 @@ int main(int argc, char* argv[]) {
f += 10;
i++;
// TODO: Fake CF data
CrazyflieData crazyfly;
crazyfly.crazyflieName = "CF1";
crazyfly.x = 0;
crazyfly.y = 0;
crazyfly.z = 0;
crazyfly.yaw = 3.14159/600 * f;
viconData.crazyflies.push_back(crazyfly);
viconDataPublisher.publish(viconData); // testing data
}
#else
......
Supports Markdown
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