Commit 89c5ca03 authored by roangel's avatar roangel
Browse files

Added messages for unlabeled markers

parent ec64ec5c
......@@ -94,6 +94,8 @@ qt5_wrap_cpp(SRC_MOC_HDRS ${SRC_HDRS_QOBJECT})
add_message_files(
FILES
UnlabeledMarker.msg
UnlabeledMarkersArray.msg
ViconData.msg
MotorCommand.msg
AngleCommand.msg
......
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.0.2, 2017-05-02T17:57:24. -->
<!-- Written by QtCreator 4.0.2, 2017-05-04T16:41:44. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
......
......@@ -9,6 +9,8 @@
#ifdef CATKIN_MAKE
#include "rosNodeThread.h"
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/UnlabeledMarkersArray.h"
#endif
#include "ui_mainguiwindow.h"
......@@ -30,7 +32,6 @@ struct setpoint
double y;
double z;
double yaw;
};
class CSetpointQueue
......@@ -97,8 +98,9 @@ private slots:
void on_checkBox_vicon_highlight_markers_toggled(bool checked);
void setPosMarker(double x, double y);
#ifdef CATKIN_MAKE
void setPosMarkers(const UnlabeledMarkersArray::ConstPtr&);
#endif
private:
Ui::MainGUIWindow *ui;
......
......@@ -12,6 +12,8 @@
#include <ros/ros.h>
#include <ros/network.h>
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/UnlabeledMarkersArray.h"
using namespace d_fall_pps;
......@@ -23,13 +25,15 @@ public:
bool init();
void messageCallback(const ViconData& data);
// void messageCallback(const ViconData& data);
void messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg);
signals:
// void newViconData(double, double, double, double, double, double);
void newViconData(double, double);
// void newViconData(double, double);
void newViconData(const UnlabeledMarkersArray::ConstPtr& p_msg);
public slots:
void run();
......
......@@ -23,7 +23,7 @@ MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
ui(new Ui::MainGUIWindow)//,
// _rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData")
{
_rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData");
_rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/UnlabeledMarkersArray");
ui->setupUi(this);
_init();
......@@ -66,9 +66,9 @@ void MainGUIWindow::_init()
scene = new myGraphicsScene(ui->frame_drawing);
scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
marker = new Marker(1 * FROM_METERS_TO_UNITS, 1 * FROM_METERS_TO_UNITS);
marker = new Marker(1 * FROM_METERS_TO_UNITS, - 1 * FROM_METERS_TO_UNITS);
// marker->setPos(0,0);
setPosMarker(1, 1);
// setPosMarkers(1, 1);
// scene->addItem(marker);
ui->graphicsView->setScene(scene);
......@@ -83,14 +83,16 @@ void MainGUIWindow::_init()
ui->checkBox_vicon_highlight_markers->setEnabled(false);
#ifdef CATKIN_MAKE
_rosNodeThread->init();
QObject::connect(_rosNodeThread, SIGNAL(newViconData(double, double)), this, SLOT(setPosMarker(double, double)));
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const UnlabeledMarkersArray::ConstPtr&)), this, SLOT(setPosMarkers(const UnlabeledMarkersArray::ConstPtr&)));
#endif
}
void MainGUIWindow::setPosMarker(double x, double y)
#ifdef CATKIN_MAKE
void MainGUIWindow::setPosMarkers(const UnlabeledMarkersArray::ConstPtr&)
{
marker->setPosMarker(scene->mapFromWorldToScene(QPointF(FROM_METERS_TO_UNITS * x, FROM_METERS_TO_UNITS * y)));
// marker->setPosMarker(scene->mapFromWorldToScene(QPointF(FROM_METERS_TO_UNITS * x, FROM_METERS_TO_UNITS * y)));
}
#endif
void MainGUIWindow::on_removeTable_clicked()
......
......@@ -43,23 +43,34 @@ bool rosNodeThread::init()
return true;
} // set up the thread
void rosNodeThread::messageCallback(const ViconData& data) // When a message arrives to the topic, this callback is executed
void rosNodeThread::messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg) // When a message arrives to the topic, this callback is executed
{
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 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);
for(int i = 0; i < p_msg->markers.size(); i++)
{
const UnlabeledMarker &marker = p_msg->markers[i];
emit newViconData(p_msg);
ROS_INFO_STREAM("index: " << marker.index << " x: " << marker.x <<
" y: " << marker.y << " z: " << marker.z);
}
}
// void rosNodeThread::messageCallback(const ViconData& data) // When a message arrives to the topic, this callback is executed
// {
// 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 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()
{
ros::Rate loop_rate(100);
......
uint32 index
float64 x
float64 y
float64 z
UnlabeledMarker[] markers
\ No newline at end of file
......@@ -30,6 +30,8 @@
#include "DataStreamClient.h"
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/UnlabeledMarkersArray.h"
using namespace ViconDataStreamSDK::CPP;
using namespace d_fall_pps;
......@@ -41,26 +43,46 @@ int main(int argc, char* argv[]) {
ros::Time::init();
ros::Publisher viconDataPublisher =
nodeHandle.advertise<ViconData>("ViconData", 1);
nodeHandle.advertise<ViconData>("ViconData", 1);
//publish something random if no viconData is available for testing
ros::Publisher unlabeledMarkersPublisher =
nodeHandle.advertise<UnlabeledMarkersArray>("UnlabeledMarkersArray", 1);
ViconData viconData;
float f = 0;
int i = 0;
while(ros::ok())
// while(ros::ok())
while(true)
{
if(i % 1000 == 0)
{
ROS_INFO("iteration #%d",i);
}
viconData.x = f;
viconDataPublisher.publish(viconData);
// Testing piece of code
UnlabeledMarker marker;
UnlabeledMarkersArray markersArray;
marker.index = 0;
marker.x = f;
marker.y = 0;
marker.z = 0;
markersArray.markers.push_back(marker);
marker.index = 1;
marker.x = 0;
marker.y = f;
marker.z = 0;
markersArray.markers.push_back(marker);
unlabeledMarkersPublisher.publish(markersArray);
ros::Duration(0.1).sleep();
f += 0.01;
i++;
}
//the code will not go further than here if testing without real ViconData
//the testing code will not go further than here if testing without real ViconData
Client client;
......@@ -114,6 +136,29 @@ int main(int argc, char* argv[]) {
ros::Duration(0.001).sleep();
}
// Unlabeled markers, for GUI
unsigned int unlabeledMarkerCount = client.GetUnlabeledMarkerCount().MarkerCount;
UnlabeledMarker marker;
UnlabeledMarkersArray markersArray;
for(int unlabeledMarkerIndex = 0; i < unlabeledMarkerCount; i++)
{
Output_GetUnlabeledMarkerGlobalTranslation OutputTranslation =
client.GetUnlabeledMarkerGlobalTranslation(unlabeledMarkerIndex);
marker.index = unlabeledMarkerIndex;
marker.x = OutputTranslation.Translation[0];
marker.y = OutputTranslation.Translation[1];
marker.z = OutputTranslation.Translation[2];
markersArray.markers.push_back(marker);
}
unlabeledMarkersPublisher.publish(markersArray);
unsigned int subjectCount = client.GetSubjectCount().SubjectCount;
// //Process the data and publish on topic
......
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