Commit 90f246d2 authored by roangel's avatar roangel
Browse files

Now, markers we receive by vicon publisher are automatically drawn in screen

parent 8c17ef24
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.0.2, 2017-05-04T16:41:44. -->
<!-- Written by QtCreator 4.0.2, 2017-05-11T12:29:20. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
......
......@@ -99,7 +99,7 @@ private slots:
void on_checkBox_vicon_highlight_markers_toggled(bool checked);
#ifdef CATKIN_MAKE
void setPosMarkers(const UnlabeledMarkersArray::ConstPtr& p_msg);
void setPosMarkers(const ptrToMessage& p_msg);
#endif
private:
......
......@@ -17,6 +17,11 @@
using namespace d_fall_pps;
typedef UnlabeledMarkersArray::ConstPtr ptrToMessage;
Q_DECLARE_METATYPE(ptrToMessage)
class rosNodeThread : public QObject {
Q_OBJECT
public:
......@@ -33,7 +38,7 @@ signals:
// void newViconData(double, double, double, double, double, double);
// void newViconData(double, double);
void newViconData(const UnlabeledMarkersArray::ConstPtr& p_msg);
void newViconData(const ptrToMessage& p_msg);
public slots:
void run();
......
......@@ -8,6 +8,7 @@
#include <QDoubleSpinBox>
#include <QTextEdit>
#include <QString>
#include <QMetaType>
#include <string>
......@@ -79,40 +80,53 @@ void MainGUIWindow::_init()
QObject::connect(scene, SIGNAL(numTablePiecesChanged(int)), this, SLOT(handleTablePiecesNumChanged(int)));
ui->checkBox_vicon_highlight_markers->setEnabled(false);
#ifdef CATKIN_MAKE
_rosNodeThread->init();
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const UnlabeledMarkersArray::ConstPtr&)), this, SLOT(setPosMarkers(const UnlabeledMarkersArray::ConstPtr&)));
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(setPosMarkers(const ptrToMessage&)));
#endif
}
#ifdef CATKIN_MAKE
void MainGUIWindow::setPosMarkers(const UnlabeledMarkersArray::ConstPtr& p_msg)
void MainGUIWindow::setPosMarkers(const ptrToMessage& p_msg) //connected to newViconData, from node
{
// TODO: Create/edit vector of markers depending on p_msg.
// Delete previous markers and plot new? or change position of related indexes?
if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared
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++)
{
scene->removeItem(markers_vector[i]);
scene->removeItem(markers_vector[i]); // remove objects from scene
ROS_INFO_STREAM("element index: " << i << " removed");
}
markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end());
markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end()); //delete them
}
ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());
for(int i = 0; i < p_msg->markers.size(); i++) // here, or new markers message is equal to current messages, or greater (some new markers)
{
if(i >= markers_vector.size()) //some new markers coming
{
markers_vector[i] = new Marker(p_msg->markers[i].x * FROM_METERS_TO_UNITS, p_msg->markers[i].y * FROM_METERS_TO_UNITS);
scene->addItem(markers_vector[i]);
ROS_INFO_STREAM("element index: " << i << " added");
Marker* tmp_p_marker = new Marker(p_msg->markers[i].x * FROM_METERS_TO_UNITS, p_msg->markers[i].y * FROM_METERS_TO_UNITS);
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..
{
scene->addItem(markers_vector[i]);
if(ui->checkBox_vicon_highlight_markers->checkState() == Qt::Checked)
{
markers_vector[i]->setHighlighted();
}
}
}
else
{
ROS_INFO_STREAM("element index: " << i << " moved, already existed");
markers_vector[i]->setPosMarker(QPointF(p_msg->markers[i].x * FROM_METERS_TO_UNITS, p_msg->markers[i].y * FROM_METERS_TO_UNITS));
}
}
// marker->setPosMarker(scene->mapFromWorldToScene(QPointF(FROM_METERS_TO_UNITS * x, FROM_METERS_TO_UNITS * y)));
}
#endif
......@@ -269,16 +283,19 @@ 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);
scene->addItem(marker);
for(int i = 0; i < markers_vector.size(); i++)
{
scene->addItem(markers_vector[i]);
}
ui->checkBox_vicon_highlight_markers->setCheckable(true);
ui->checkBox_vicon_highlight_markers->setEnabled(true);
}
else
{
scene->removeItem(marker);
// marker->setParentItem(NULL);
// delete marker;
for(int i = 0; i < markers_vector.size(); i++)
{
scene->removeItem(markers_vector[i]);
}
ui->checkBox_vicon_highlight_markers->setChecked(false);
ui->checkBox_vicon_highlight_markers->setCheckable(false);
ui->checkBox_vicon_highlight_markers->setEnabled(false);
......@@ -289,10 +306,16 @@ void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
{
if(checked)
{
marker->setHighlighted();
for(int i = 0; i < markers_vector.size(); i++)
{
markers_vector[i]->setHighlighted();
}
}
else
{
marker->clearHighlighted();
for(int i = 0; i < markers_vector.size(); i++)
{
markers_vector[i]->clearHighlighted();
}
}
}
#include "rosNodeThread.h"
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * topic, QObject* parent)
: QObject(parent),
m_Init_argc(argc),
......@@ -45,13 +46,7 @@ bool rosNodeThread::init()
void rosNodeThread::messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg) // When a message arrives to the topic, this callback is executed
{
for(int i = 0; i < p_msg->markers.size(); i++)
{
const UnlabeledMarker &marker = p_msg->markers[i];
ROS_INFO_STREAM("index: " << marker.index << " x: " << marker.x <<
" y: " << marker.y << " z: " << marker.z);
}
emit newViconData(p_msg);
emit newViconData(p_msg); //pass the message to other places
}
// void rosNodeThread::messageCallback(const ViconData& data) // When a message arrives to the topic, this callback is executed
......
......@@ -75,6 +75,16 @@ int main(int argc, char* argv[]) {
marker.z = 0;
markersArray.markers.push_back(marker);
if(i > 50 && i < 100)
{
marker.index = 2;
marker.x = f;
marker.y = f;
marker.z = 0;
markersArray.markers.push_back(marker);
}
unlabeledMarkersPublisher.publish(markersArray);
ros::Duration(0.1).sleep();
f += 0.01;
......
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