/* This file is part of RGBDSLAM.
*
* RGBDSLAM 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.
*
* RGBDSLAM 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 RGBDSLAM. If not, see .
*/
#include
#include
#include
#include
#include
#include "ros_service_ui.h"
#include
#include "ros/ros.h"
RosUi::RosUi(const char* service_namespace) : filename("quicksave.pcd"), record_on(false)
{
createActions(service_namespace);
this->pause_on = ParameterServer::instance()->get("start_paused");
}
void RosUi::resetCmd() {
ROS_INFO("Graph Reset");
Q_EMIT reset();
ROS_INFO("A fresh new graph is waiting");
}
void RosUi::quickSaveAll() {
Q_EMIT saveAllClouds(filename);
ROS_INFO("Saving Whole Model to %s", qPrintable(filename));
}
void RosUi::saveAll() {
Q_EMIT saveAllClouds(filename);
ROS_INFO("Saving Whole Model to %s", qPrintable(filename));
}
void RosUi::saveIndividual() {
QString tmpfilename(filename);
tmpfilename.remove(".pcd", Qt::CaseInsensitive);
tmpfilename.remove(".ply", Qt::CaseInsensitive);
Q_EMIT saveIndividualClouds(filename);
ROS_INFO("Saving Model Node-Wise");
}
void RosUi::sendAll() {
Q_EMIT sendAllClouds();
ROS_INFO("Sending Whole Model");
}
void RosUi::sendFinished() {
ROS_INFO("Finished Sending");
}
void RosUi::getOneFrameCmd() {
Q_EMIT getOneFrame();
ROS_INFO("Getting a single frame");
}
void RosUi::deleteLastFrameCmd() {
Q_EMIT deleteLastFrame();
ROS_INFO("Deleting the last node from the graph");
}
void RosUi::bagRecording(bool _record_on) {
if(this->record_on == _record_on){
return;
}
this->record_on = _record_on;
Q_EMIT toggleBagRecording();
if(_record_on) {
ROS_INFO("Recording Bagfile.");
} else {
ROS_INFO("Stopped Recording.");
}
}
void RosUi::pause(bool _pause_on) {
if(this->pause_on == _pause_on){
return;
}
this->pause_on = _pause_on;
Q_EMIT togglePause();
if(!_pause_on) {
ROS_INFO("Processing.");
} else {
ROS_INFO("Stopped processing.");
}
}
void RosUi::setMax(float val){
Q_EMIT setMaxDepth(val/100.0);
}
void RosUi::createActions(const char* service_namespace) {
//srv_reset der bei triggern resetCmd() aufruft
ros::NodeHandle n(service_namespace);
server = n.advertiseService("ros_ui", &RosUi::services, this);
server_b = n.advertiseService("ros_ui_b", &RosUi::services_b, this);
server_f = n.advertiseService("ros_ui_f", &RosUi::services_f, this);
}
bool RosUi::services(rgbdslam::rgbdslam_ros_ui::Request &req,
rgbdslam::rgbdslam_ros_ui::Response &res )
{
if (req.comand == "reset" ){ resetCmd(); }
else if(req.comand == "quick_save" ){ quickSaveAll(); }
else if(req.comand == "save_all" ){ saveAll(); }
else if(req.comand == "save_trajectory"){ Q_EMIT saveTrajectory("trajectory"); }
else if(req.comand == "save_individual"){ saveIndividual(); }
else if(req.comand == "send_all" ){ sendAll(); }
else if(req.comand == "frame" ){ getOneFrame(); }
else if(req.comand == "delete_frame" ){ deleteLastFrame(); }
else if(req.comand == "delete_frame" ){ Q_EMIT optimizeGraph(); }
else{
ROS_ERROR("Valid commands are: {reset, quick_save, save_all, save_individual, send_all, delete_frame}");
return false;
}
return true;
}
bool RosUi::services_b(rgbdslam::rgbdslam_ros_ui_b::Request &req,
rgbdslam::rgbdslam_ros_ui_b::Response &res )
{
if (req.comand == "pause" ){ pause(req.value); }
else if(req.comand == "record"){ bagRecording(req.value); }
else{
ROS_ERROR("Valid commands are: {pause, record}");
return false;
}
return true;
}
bool RosUi::services_f(rgbdslam::rgbdslam_ros_ui_f::Request &req,
rgbdslam::rgbdslam_ros_ui_f::Response &res )
{
if(req.comand == "set_max"){
setMax(req.value);
return true;
}
else{
ROS_ERROR("Command is set_max");
return false;
}
}