rosNodeThread.cpp 4.26 KB
Newer Older
roangel's avatar
roangel committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//    Creates a thread that runs the ros node.
//
//    Copyright (C) 2017  Angel Romero
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

18
19
#include "rosNodeThread.h"

20
#include "d_fall_pps/CMRead.h"
21
22
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
23

24

25
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name,  QObject* parent)
26
27
    :   QObject(parent),
        m_Init_argc(argc),
28
        m_pInit_argv(pArgv),
29
        m_node_name(node_name)
30
{
31
    /** Constructor for the node thread **/
32
33
34
35
}

rosNodeThread::~rosNodeThread()
{
tiagos's avatar
tiagos committed
36
37
38
39
40
    // join the thread
    m_pThread->exit();
    // delete the nodehandle so ros can shutdown cleanly
    delete nh;

41
42
43
44
    if (ros::isStarted())
    {
        ros::shutdown();
        ros::waitForShutdown();
45
    } // end if
46

47
} // end destructor
48
49
50
51
52
53

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

54
    connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
55
    ros::init(m_Init_argc, m_pInit_argv, m_node_name); // my_GUI is the name of this node
56
57

    if (!ros::master::check())
roangel's avatar
roangel committed
58
59
    {
        ROS_ERROR("No master found. Please make sure that there is a master roscore running");
60
        return false;           // do not start without ros.
roangel's avatar
roangel committed
61
    }
62
63
64

    ros::start();
    ros::Time::init();
tiagos's avatar
tiagos committed
65
66
67
    //ros::NodeHandle nh("~");

    nh = new ros::NodeHandle("~");
68

tiagos's avatar
tiagos committed
69
70
71
    m_vicon_subscriber = nh->subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
    studentID_subscriber = nh->subscribe("/TeacherService/studentIDs", 1, &rosNodeThread::studentIDCallback, this);
    positionData_subscriber = nh->subscribe("/TeacherService/positionData", 1, &rosNodeThread::positionDataCallback, this);
72

73
    // clients for db services:
tiagos's avatar
tiagos committed
74
75
76
    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);
77

78
79
80
81
    m_pThread->start();
    return true;
} // set up the thread

82
83
84
85
86
void rosNodeThread::studentIDCallback(const std_msgs::Int32MultiArray &ids)
{
    emit newStudentIDs(ids);
}

muelmarc's avatar
merging    
muelmarc committed
87
void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
88
{
89
    emit newViconData(p_msg);   //pass the message to other places
90
91
}

92
93
94
95
96
void rosNodeThread::positionDataCallback(const CrazyfliePositionData& data)
{
    emit newPositionData(data);
}

roangel's avatar
roangel committed
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
// 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);
// }

114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
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.
}