Quadrotor

/* 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 <http://www.gnu.org/licenses/>.

 */

#include <QtGui>

#include <QPixmap>

#include <QFont>

#include <QIcon>

#include <QKeySequence>

#include "ros_service_ui.h"

#include <limits>

#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<bool>("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;

    }

}