rosNodeThread.cpp 5.17 KB
Newer Older
1
2
3
4
5
6
//    Copyright (C) 2018, ETH Zurich, D-ITET
//          Yvan Bosshard           byvan       @ee.ethz.ch
//          Michael Rogenmoser      michaero    @ee.ethz.ch
//          Tiago Salzmann          tiagos      @ee.ethz.ch
//    
//    Adapted from rosNodeThread of Angel Romero.
roangel's avatar
roangel committed
7
//
8
9
10
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
roangel's avatar
roangel committed
11
12
13
//    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.
14
15
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
roangel's avatar
roangel committed
16
17
18
//    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.
19
//    
roangel's avatar
roangel committed
20
//    You should have received a copy of the GNU General Public License
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    Creates a thread that runs the ros node.
//
//    ----------------------------------------------------------------------------------

roangel's avatar
roangel committed
37

38
39
#include "rosNodeThread.h"

40
#include "d_fall_pps/CMRead.h"
41
42
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
43

44

45
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name,  QObject* parent)
46
47
    :   QObject(parent),
        m_Init_argc(argc),
48
        m_pInit_argv(pArgv),
49
        m_node_name(node_name)
50
{
51
    /** Constructor for the node thread **/
52
53
54
55
}

rosNodeThread::~rosNodeThread()
{
tiagos's avatar
tiagos committed
56
57
58
59
60
    // join the thread
    m_pThread->exit();
    // delete the nodehandle so ros can shutdown cleanly
    delete nh;

61
62
63
64
    if (ros::isStarted())
    {
        ros::shutdown();
        ros::waitForShutdown();
65
    } // end if
66

67
} // end destructor
68
69
70
71
72
73

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

74
    connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
75
    ros::init(m_Init_argc, m_pInit_argv, m_node_name); // my_GUI is the name of this node
76
77

    if (!ros::master::check())
roangel's avatar
roangel committed
78
79
    {
        ROS_ERROR("No master found. Please make sure that there is a master roscore running");
80
        return false;           // do not start without ros.
roangel's avatar
roangel committed
81
    }
82
83
84

    ros::start();
    ros::Time::init();
tiagos's avatar
tiagos committed
85
86

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

tiagos's avatar
tiagos committed
88
89
90
    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);
91

92
    // clients for db services:
tiagos's avatar
tiagos committed
93
94
95
    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);
96

97
98
99
100
    m_pThread->start();
    return true;
} // set up the thread

101
102
103
104
105
void rosNodeThread::studentIDCallback(const std_msgs::Int32MultiArray &ids)
{
    emit newStudentIDs(ids);
}

muelmarc's avatar
merging    
muelmarc committed
106
void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
107
{
108
    emit newViconData(p_msg);   //pass the message to other places
109
110
}

111
112
113
114
115
void rosNodeThread::positionDataCallback(const CrazyfliePositionData& data)
{
    emit newPositionData(data);
}

roangel's avatar
roangel committed
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
// 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);
// }

133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
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.
}