rosNodeThread.cpp 2.69 KB
Newer Older
1
2
#include "rosNodeThread.h"

3
4
5
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * topic, QObject* parent)
    :   QObject(parent),
        m_Init_argc(argc),
6
7
8
9
        m_pInit_argv(pArgv),
        m_topic(topic)

{
10
    /** Constructor for the node thread **/
11
12
13
14
15
16
17
18
}

rosNodeThread::~rosNodeThread()
{
    if (ros::isStarted())
    {
        ros::shutdown();
        ros::waitForShutdown();
19
    } // end if
20
21

    m_pThread->wait();
22
} // end destructor
23
24
25
26
27
28

bool rosNodeThread::init()
{
    m_pThread = new QThread();
    this->moveToThread(m_pThread); // QObject method

29
30
    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
31
32
33
34
35
36
37
38
39

    if (!ros::master::check())
        return false;           // do not start without ros.

    ros::start();
    ros::Time::init();
    ros::NodeHandle nh("~");

    // sim_velocity  = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
40
    m_vicon_subscriber = nh.subscribe(m_topic, 100, &rosNodeThread::messageCallback, this);
41
42
43
44
45

    m_pThread->start();
    return true;
} // set up the thread

roangel's avatar
roangel committed
46
void rosNodeThread::messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg) // When a message arrives to the topic, this callback is executed
47
{
roangel's avatar
roangel committed
48
49
50
51
52
53
54
    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);
    }
55
56
}

roangel's avatar
roangel committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
// 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);
// }

74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
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();

        // sim_velocity.publish(cmd_msg);
        ros::spinOnce();
        loop_rate.sleep();
        delete pMutex;
    } // do ros things.
}