Commit 141188c0 authored by roangel's avatar roangel
Browse files

now student GUI creates a new node in a new Thread. Node name: student_GUI

parent 6e182532
......@@ -77,6 +77,7 @@ qt5_add_resources(MY_RESOURCE_FILE_RCC ${MY_RESOURCE_FILE_QRC})
set(SRC_HDRS_QOBJECT_STUDENT_GUI
${STUDENT_GUI_LIB_PATH_INC}/MainWindow.h
${STUDENT_GUI_LIB_PATH_INC}/rosNodeThread.h
)
# StudentGUI -- wrap UI file and QOBJECT files
......@@ -264,6 +265,7 @@ set(MY_CPP_SOURCES_GUI # compilation of sources
set(MY_CPP_SOURCES_STUDENT_GUI # compilation of sources
${STUDENT_GUI_LIB_PATH_SRC}/MainWindow.cpp
${STUDENT_GUI_LIB_PATH_SRC}/main.cpp
${STUDENT_GUI_LIB_PATH_SRC}/rosNodeThread.cpp
)
......
......@@ -25,7 +25,7 @@ Q_DECLARE_METATYPE(ptrToMessage)
class rosNodeThread : public QObject {
Q_OBJECT
public:
explicit rosNodeThread(int argc, char **pArgv, const char * topic, QObject *parent = 0);
explicit rosNodeThread(int argc, char **pArgv, const char * node_name, QObject *parent = 0);
virtual ~rosNodeThread();
bool init();
......@@ -37,7 +37,6 @@ public:
ros::ServiceClient m_update_db_client;
ros::ServiceClient m_command_db_client;
signals:
void newViconData(const ptrToMessage& p_msg);
......@@ -48,7 +47,7 @@ public slots:
private:
int m_Init_argc;
char** m_pInit_argv;
const char * m_topic;
const char * m_node_name;
QThread * m_pThread;
......
......@@ -35,7 +35,7 @@ MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
ui(new Ui::MainGUIWindow)
{
#ifdef CATKIN_MAKE
_rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData");
_rosNodeThread = new rosNodeThread(argc, argv, "my_GUI");
#endif
ui->setupUi(this);
_init();
......
......@@ -5,12 +5,11 @@
#include "d_fall_pps/CMCommand.h"
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * topic, QObject* parent)
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name, QObject* parent)
: QObject(parent),
m_Init_argc(argc),
m_pInit_argv(pArgv),
m_topic(topic)
m_node_name(node_name)
{
/** Constructor for the node thread **/
}
......@@ -32,7 +31,7 @@ bool rosNodeThread::init()
this->moveToThread(m_pThread); // QObject method
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
ros::init(m_Init_argc, m_pInit_argv, m_node_name); // my_GUI is the name of this node
if (!ros::master::check())
return false; // do not start without ros.
......@@ -41,7 +40,7 @@ bool rosNodeThread::init()
ros::Time::init();
ros::NodeHandle nh("~");
m_vicon_subscriber = nh.subscribe(m_topic, 100, &rosNodeThread::messageCallback, this);
m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
// clients for db services:
m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
......
......@@ -3,6 +3,9 @@
#include <QMainWindow>
#include "rosNodeThread.h"
namespace Ui {
class MainWindow;
}
......@@ -12,11 +15,16 @@ class MainWindow : public QMainWindow
Q_OBJECT
public:
explicit MainWindow(QWidget *parent = 0);
explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
~MainWindow();
private slots:
void updateNewViconData(const ptrToMessage& p_msg);
private:
Ui::MainWindow *ui;
rosNodeThread* m_rosNodeThread;
};
#endif // MAINWINDOW_H
#ifndef ___ROSNODETHREAD_H___
#define ___ROSNODETHREAD_H___
#include <QtCore>
#include <QThread>
#include <QStringList>
#include <stdlib.h>
#include <QMutex>
#include <iostream>
#include "assert.h"
#include <ros/ros.h>
#include <ros/network.h>
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/ViconData.h"
using namespace d_fall_pps;
typedef ViconData::ConstPtr ptrToMessage;
Q_DECLARE_METATYPE(ptrToMessage)
class rosNodeThread : public QObject {
Q_OBJECT
public:
explicit rosNodeThread(int argc, char **pArgv, const char * node_name, QObject *parent = 0);
virtual ~rosNodeThread();
bool init();
// void messageCallback(const ViconData& data);
void messageCallback(const ptrToMessage& p_msg);
ros::ServiceClient m_read_db_client;
ros::ServiceClient m_update_db_client;
ros::ServiceClient m_command_db_client;
signals:
void newViconData(const ptrToMessage& p_msg);
public slots:
void run();
private:
int m_Init_argc;
char** m_pInit_argv;
const char * m_node_name;
QThread * m_pThread;
ViconData m_vicon_data;
ros::Subscriber m_vicon_subscriber;
// ros::Publisher sim_velocity;
};
#endif
#include "MainWindow.h"
#include "ui_MainWindow.h"
MainWindow::MainWindow(QWidget *parent) :
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
ui->setupUi(this);
m_rosNodeThread->init();
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
}
MainWindow::~MainWindow()
{
delete ui;
}
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
}
......@@ -4,8 +4,7 @@
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
MainWindow w;
MainWindow w(argc, argv);
w.show();
return a.exec();
}
#include "rosNodeThread.h"
#include "d_fall_pps/CMRead.h"
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name, QObject* parent)
: QObject(parent),
m_Init_argc(argc),
m_pInit_argv(pArgv),
m_node_name(node_name)
{
/** Constructor for the node thread **/
}
rosNodeThread::~rosNodeThread()
{
if (ros::isStarted())
{
ros::shutdown();
ros::waitForShutdown();
} // end if
m_pThread->wait();
} // end destructor
bool rosNodeThread::init()
{
m_pThread = new QThread();
this->moveToThread(m_pThread); // QObject method
connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
ros::init(m_Init_argc, m_pInit_argv, m_node_name); // student_GUI is the name of this node
if (!ros::master::check())
{
ROS_ERROR("Master not found, please check teacher computer is running and try again");
return false; // do not start without ros.
}
ros::start();
ros::Time::init();
ros::NodeHandle nh("~");
m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
// clients for db services:
m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
m_update_db_client = nh.serviceClient<CMUpdate>("/CentralManagerService/Update", false);
m_command_db_client = nh.serviceClient<CMCommand>("/CentralManagerService/Command", false);
m_pThread->start();
return true;
} // set up the thread
void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
{
emit newViconData(p_msg); //pass the message to other places
}
void rosNodeThread::run()
{
ros::Rate loop_rate(100);
QMutex * pMutex;
while (ros::ok())
{
pMutex = new QMutex();
// geometry_msgs::Twist cmd_msg;
pMutex->lock();
// cmd_msg.linear.x = m_speed;
// cmd_msg.angular.z = m_angle;
pMutex->unlock();
ROS_INFO("RUNNING");
// sim_velocity.publish(cmd_msg);
ros::spinOnce();
loop_rate.sleep();
delete pMutex;
} // do ros things.
}
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