root / rgbdslam / src / ros_service_ui.cpp @ 9240aaa3
History | View | Annotate | Download (4.54 KB)
1 | 9240aaa3 | Alex | /* 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 | #include <QtGui> |
||
17 | #include <QPixmap> |
||
18 | #include <QFont> |
||
19 | #include <QIcon> |
||
20 | #include <QKeySequence> |
||
21 | #include "ros_service_ui.h" |
||
22 | #include <limits> |
||
23 | #include "ros/ros.h" |
||
24 | |||
25 | RosUi::RosUi(const char* service_namespace) : filename("quicksave.pcd"), record_on(false) |
||
26 | { |
||
27 | createActions(service_namespace); |
||
28 | this->pause_on = ParameterServer::instance()->get<bool>("start_paused"); |
||
29 | } |
||
30 | |||
31 | void RosUi::resetCmd() {
|
||
32 | ROS_INFO("Graph Reset");
|
||
33 | Q_EMIT reset(); |
||
34 | ROS_INFO("A fresh new graph is waiting");
|
||
35 | } |
||
36 | |||
37 | void RosUi::quickSaveAll() {
|
||
38 | Q_EMIT saveAllClouds(filename); |
||
39 | ROS_INFO("Saving Whole Model to %s", qPrintable(filename));
|
||
40 | } |
||
41 | |||
42 | void RosUi::saveAll() {
|
||
43 | Q_EMIT saveAllClouds(filename); |
||
44 | ROS_INFO("Saving Whole Model to %s", qPrintable(filename));
|
||
45 | } |
||
46 | |||
47 | void RosUi::saveIndividual() {
|
||
48 | QString tmpfilename(filename); |
||
49 | tmpfilename.remove(".pcd", Qt::CaseInsensitive);
|
||
50 | tmpfilename.remove(".ply", Qt::CaseInsensitive);
|
||
51 | Q_EMIT saveIndividualClouds(filename); |
||
52 | ROS_INFO("Saving Model Node-Wise");
|
||
53 | } |
||
54 | |||
55 | void RosUi::sendAll() {
|
||
56 | Q_EMIT sendAllClouds(); |
||
57 | ROS_INFO("Sending Whole Model");
|
||
58 | } |
||
59 | |||
60 | void RosUi::sendFinished() {
|
||
61 | ROS_INFO("Finished Sending");
|
||
62 | } |
||
63 | |||
64 | void RosUi::getOneFrameCmd() {
|
||
65 | Q_EMIT getOneFrame(); |
||
66 | ROS_INFO("Getting a single frame");
|
||
67 | } |
||
68 | |||
69 | void RosUi::deleteLastFrameCmd() {
|
||
70 | Q_EMIT deleteLastFrame(); |
||
71 | ROS_INFO("Deleting the last node from the graph");
|
||
72 | } |
||
73 | |||
74 | void RosUi::bagRecording(bool _record_on) { |
||
75 | if(this->record_on == _record_on){ |
||
76 | return;
|
||
77 | } |
||
78 | this->record_on = _record_on;
|
||
79 | Q_EMIT toggleBagRecording(); |
||
80 | if(_record_on) {
|
||
81 | ROS_INFO("Recording Bagfile.");
|
||
82 | } else {
|
||
83 | ROS_INFO("Stopped Recording.");
|
||
84 | } |
||
85 | } |
||
86 | |||
87 | void RosUi::pause(bool _pause_on) { |
||
88 | if(this->pause_on == _pause_on){ |
||
89 | return;
|
||
90 | } |
||
91 | this->pause_on = _pause_on;
|
||
92 | Q_EMIT togglePause(); |
||
93 | if(!_pause_on) {
|
||
94 | ROS_INFO("Processing.");
|
||
95 | } else {
|
||
96 | ROS_INFO("Stopped processing.");
|
||
97 | } |
||
98 | } |
||
99 | |||
100 | void RosUi::setMax(float val){ |
||
101 | Q_EMIT setMaxDepth(val/100.0); |
||
102 | } |
||
103 | |||
104 | void RosUi::createActions(const char* service_namespace) { |
||
105 | //srv_reset der bei triggern resetCmd() aufruft
|
||
106 | ros::NodeHandle n(service_namespace); |
||
107 | server = n.advertiseService("ros_ui", &RosUi::services, this); |
||
108 | server_b = n.advertiseService("ros_ui_b", &RosUi::services_b, this); |
||
109 | server_f = n.advertiseService("ros_ui_f", &RosUi::services_f, this); |
||
110 | } |
||
111 | |||
112 | bool RosUi::services(rgbdslam::rgbdslam_ros_ui::Request &req,
|
||
113 | rgbdslam::rgbdslam_ros_ui::Response &res ) |
||
114 | { |
||
115 | if (req.comand == "reset" ){ resetCmd(); } |
||
116 | else if(req.comand == "quick_save" ){ quickSaveAll(); } |
||
117 | else if(req.comand == "save_all" ){ saveAll(); } |
||
118 | else if(req.comand == "save_trajectory"){ Q_EMIT saveTrajectory("trajectory"); } |
||
119 | else if(req.comand == "save_individual"){ saveIndividual(); } |
||
120 | else if(req.comand == "send_all" ){ sendAll(); } |
||
121 | else if(req.comand == "frame" ){ getOneFrame(); } |
||
122 | else if(req.comand == "delete_frame" ){ deleteLastFrame(); } |
||
123 | else if(req.comand == "delete_frame" ){ Q_EMIT optimizeGraph(); } |
||
124 | else{
|
||
125 | ROS_ERROR("Valid commands are: {reset, quick_save, save_all, save_individual, send_all, delete_frame}");
|
||
126 | return false; |
||
127 | } |
||
128 | return true; |
||
129 | } |
||
130 | |||
131 | bool RosUi::services_b(rgbdslam::rgbdslam_ros_ui_b::Request &req,
|
||
132 | rgbdslam::rgbdslam_ros_ui_b::Response &res ) |
||
133 | { |
||
134 | if (req.comand == "pause" ){ pause(req.value); } |
||
135 | else if(req.comand == "record"){ bagRecording(req.value); } |
||
136 | else{
|
||
137 | ROS_ERROR("Valid commands are: {pause, record}");
|
||
138 | return false; |
||
139 | } |
||
140 | return true; |
||
141 | } |
||
142 | |||
143 | bool RosUi::services_f(rgbdslam::rgbdslam_ros_ui_f::Request &req,
|
||
144 | rgbdslam::rgbdslam_ros_ui_f::Response &res ) |
||
145 | { |
||
146 | if(req.comand == "set_max"){ |
||
147 | setMax(req.value); |
||
148 | return true; |
||
149 | } |
||
150 | else{
|
||
151 | ROS_ERROR("Command is set_max");
|
||
152 | return false; |
||
153 | } |
||
154 | } |