rosNodeThread.cpp 1.94 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
#include "rosNodeThread.h"

rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * topic)
    :	m_Init_argc(argc),
        m_pInit_argv(pArgv),
        m_topic(topic)

{
    /** Constructor for the robot 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, &QThread::started, this, &rosNodeThread::run);
    ros::init(m_Init_argc, m_pInit_argv, "GUI"); // GUI is the name of this node

    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);
    test = nh.subscribe(m_topic, 10, &rosNodeThread::messageCallback, this);

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

void rosNodeThread::messageCallback(const ViconData& data) // When a message arrives to the topic, this callback is executed
{
    QMutex * pMutex = new QMutex();

    pMutex->lock();

    // m_xPos = msg.pose.pose.position.x;
    // m_yPos = msg.pose.pose.position.y;
    // m_aPos = msg.pose.pose.orientation.w;

    pMutex->unlock();
    ROS_INFO("in viconCallback");
    qDebug("in viconCallback");
    delete pMutex;
    // Q_EMIT newPose(m_xPos, m_yPos, m_aPos);
}

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.
}