Commit 8db7c445 authored by roangel's avatar roangel
Browse files

Added support for plotting, moving and rotating crazyflies. Compiles and works perfect

parent eaf6187c
......@@ -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 \
......
......@@ -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,14 +10,14 @@
#ifdef CATKIN_MAKE
#include "rosNodeThread.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;
......@@ -106,11 +106,10 @@ private:
myGraphicsScene* scene;
void _init();
std::vector<Marker*> markers_vector;
std::vector<crazyFly*> crazyflies_vector;
#ifdef CATKIN_MAKE
rosNodeThread* _rosNodeThread;
std::vector<Marker*> markers_vector;
std::vector<crazyFly*> crazyflies_vector;
#endif
};
......
......@@ -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);
......
......@@ -134,11 +134,53 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
}
// 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);
}
}
// TODO: create a vector of pointers to CFs with all the info inside. Update this vector with new coming data
// To access this vector by name, we should maybe create a second array of strings (names), that are located at the
// same position as the information of CFs is
// 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);
}
}
}
#endif
......@@ -292,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++)
......@@ -312,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++)
......@@ -330,4 +374,5 @@ void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
markers_vector[i]->clearHighlighted();
}
}
#endif
}
......@@ -12,11 +12,7 @@ Marker::Marker(const UnlabeledMarker* p_marker_msg, QGraphicsItem * parent)
_highlighted = false;
_highlight_diameter = HIGHLIGHT_DIAMETER;
QPointF p(m_x, m_y);
_diameter = MARKER_DIAMETER; // x and y are top left coordinates
this->setPos(m_x, m_y); //where it is now, it is the center
_x_highlight = m_x - _highlight_diameter/2; // update top-left corner coordinates of highlighing circle
_y_highlight = m_y - _highlight_diameter/2;
......
......@@ -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