Statistics
| Branch: | Revision:

root / rgbdslam / src / ros_service_ui.cpp @ 9240aaa3

History | View | Annotate | Download (4.54 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
#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
}
155