root / rgbdslam / src / qtros.cpp @ 9240aaa3
History | View | Annotate | Download (1.32 KB)
1 |
/* This file is part of RGBDSLAM.
|
---|---|
2 |
*
|
3 |
* RGBDSLAM is free software: you can redistribute it and/or modify
|
4 |
* it under the terms of the GNU General Public License as published by
|
5 |
* the Free Software Foundation, either version 3 of the License, or
|
6 |
* (at your option) any later version.
|
7 |
*
|
8 |
* RGBDSLAM is distributed in the hope that it will be useful,
|
9 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
10 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
11 |
* GNU General Public License for more details.
|
12 |
*
|
13 |
* You should have received a copy of the GNU General Public License
|
14 |
* along with RGBDSLAM. If not, see <http://www.gnu.org/licenses/>.
|
15 |
*/
|
16 |
|
17 |
|
18 |
#include "qtros.h" |
19 |
|
20 |
QtROS::QtROS(int argc, char *argv[], const char* node_name) { |
21 |
std::cout << "Initializing Node...\n";
|
22 |
ros::init(argc, argv, node_name); |
23 |
n = new ros::NodeHandle(node_name); //Use node name as Ros Namespace |
24 |
ROS_INFO("Connected to roscore");
|
25 |
quitfromgui = false;
|
26 |
} |
27 |
|
28 |
void QtROS::quitNow(){
|
29 |
quitfromgui = true;
|
30 |
} |
31 |
|
32 |
void QtROS::run(){
|
33 |
ros::Rate r(30); // 30 hz. Kinect has 30hz and we are far from processing every frame anyhow. |
34 |
while(ros::ok() && !quitfromgui) {
|
35 |
ros::spinOnce(); |
36 |
r.sleep();} |
37 |
if (!quitfromgui) {
|
38 |
Q_EMIT rosQuits(); |
39 |
ROS_INFO("ROS-Node Terminated\n");
|
40 |
ros::shutdown();//Not sure if necessary
|
41 |
} |
42 |
} |