rosNodeThread.h 1.19 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
#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>
roangel's avatar
roangel committed
14
#include "d_fall_pps/UnlabeledMarker.h"
muelmarc's avatar
merging    
muelmarc committed
15
16
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/ViconData.h"
17
18
19

using namespace d_fall_pps;

muelmarc's avatar
merging    
muelmarc committed
20
typedef ViconData::ConstPtr ptrToMessage;
21
22
23
24

Q_DECLARE_METATYPE(ptrToMessage)


25
26
27
class rosNodeThread : public QObject {
	Q_OBJECT
public:
28
    explicit rosNodeThread(int argc, char **pArgv, const char * topic, QObject *parent = 0);
29
30
31
32
    virtual ~rosNodeThread();

    bool init();

roangel's avatar
roangel committed
33
    // void messageCallback(const ViconData& data);
muelmarc's avatar
merging    
muelmarc committed
34
    void messageCallback(const ptrToMessage& p_msg);
35

36
    ros::ServiceClient m_read_db_client;
37
38
    ros::ServiceClient m_update_db_client;
    ros::ServiceClient m_command_db_client;
39

40
41

signals:
roangel's avatar
roangel committed
42

43
    void newViconData(const ptrToMessage& p_msg);
44
45
46
47

public slots:
    void run();

48
49
50
51
52
53
54
private:
    int m_Init_argc;
    char** m_pInit_argv;
    const char * m_topic;

    QThread * m_pThread;

55
56
57
    ViconData m_vicon_data;

    ros::Subscriber m_vicon_subscriber;
58
59
60
61
    // ros::Publisher  sim_velocity;
};
#endif