Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (6.78 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
/* registration main:
18
 * Create 
19
 * - a Qt Application 
20
 * - a ROS Node, 
21
 * - a ROS listener, listening to and processing kinect data
22
 * - Let them communicate internally via QT Signals
23
 */
24
#include "openni_listener.h"
25
#include "qtros.h"
26
#include <QApplication>
27
#include <QObject>
28
#include "qt_gui.h"
29
#include <Eigen/Core>
30
#include "parameter_server.h"
31
#include "ros_service_ui.h"
32

    
33
//TODO:
34
//better potential-edge-selection through flann
35
//Better separation of function, communication, parameters and gui
36

    
37
///Connect Signals and Slots for the ui control
38
void ui_connections(QObject* ui, GraphManager* graph_mgr, OpenNIListener* kinect_listener){
39
    QObject::connect(ui, SIGNAL(reset()), graph_mgr, SLOT(reset()));
40
    QObject::connect(ui, SIGNAL(optimizeGraph()), graph_mgr, SLOT(optimizeGraph()));
41
    QObject::connect(ui, SIGNAL(togglePause()), kinect_listener, SLOT(togglePause()));
42
    QObject::connect(ui, SIGNAL(toggleBagRecording()), kinect_listener, SLOT(toggleBagRecording()));
43
    QObject::connect(ui, SIGNAL(getOneFrame()), kinect_listener, SLOT(getOneFrame()));
44
    QObject::connect(ui, SIGNAL(deleteLastFrame()), graph_mgr, SLOT(deleteLastFrame()));
45
    QObject::connect(ui, SIGNAL(sendAllClouds()), graph_mgr, SLOT(sendAllClouds()));
46
    QObject::connect(ui, SIGNAL(saveAllClouds(QString)), graph_mgr, SLOT(saveAllClouds(QString)));
47
    QObject::connect(ui, SIGNAL(saveIndividualClouds(QString)), graph_mgr, SLOT(saveIndividualClouds(QString)));
48
    QObject::connect(ui, SIGNAL(setMaxDepth(float)), graph_mgr, SLOT(setMaxDepth(float)));
49
    QObject::connect(ui, SIGNAL(saveTrajectory(QString)), graph_mgr, SLOT(saveTrajectory(QString)));
50
}
51

    
52
///Connect Signals and Slots only relevant for the graphical interface
53
void gui_connections(QObject* ui, GraphManager* graph_mgr, OpenNIListener* kinect_listener, GLViewer* glviewer){
54
    QObject::connect(kinect_listener, SIGNAL(newVisualImage(QImage)), ui, SLOT(setVisualImage(QImage)));
55
    QObject::connect(kinect_listener, SIGNAL(newFeatureFlowImage(QImage)), ui, SLOT(setFeatureFlowImage(QImage)));
56
    QObject::connect(kinect_listener, SIGNAL(newDepthImage(QImage)), ui, SLOT(setDepthImage(QImage)));
57
    QObject::connect(graph_mgr, SIGNAL(newTransformationMatrix(QString)), ui, SLOT(setTransformation(QString)));
58
    QObject::connect(graph_mgr, SIGNAL(sendFinished()), ui, SLOT(sendFinished()));
59
    QObject::connect(graph_mgr, SIGNAL(setGUIInfo(QString)), ui, SLOT(setInfo(QString)));
60
    QObject::connect(graph_mgr, SIGNAL(setGUIStatus(QString)), ui, SLOT(setStatus(QString)));
61
    QObject::connect(graph_mgr, SIGNAL(deleteLastNode()), ui, SLOT(deleteLastNode()));
62
    QObject::connect(ui, SIGNAL(printEdgeErrors(QString)), graph_mgr, SLOT(printEdgeErrors(QString)));
63
    QObject::connect(ui, SIGNAL(pruneEdgesWithErrorAbove(float)), graph_mgr, SLOT(pruneEdgesWithErrorAbove(float)));
64
    if (ParameterServer::instance()->get<bool>("use_glwidget")) {
65
            QObject::connect(graph_mgr, SIGNAL(setPointCloud(pointcloud_type *, QMatrix4x4)), ui, SLOT(addPointCloud(pointcloud_type *, QMatrix4x4))); //, Qt::DirectConnection);
66
            QObject::connect(graph_mgr, SIGNAL(updateTransforms(QList<QMatrix4x4>*)), ui, SLOT(updateTransforms(QList<QMatrix4x4>*)));
67
            QObject::connect(graph_mgr, SIGNAL(setGraphEdges(QList<QPair<int, int> >*)), ui, SLOT(setGraphEdges(QList<QPair<int, int> >*)));
68
            QObject::connect(graph_mgr, SIGNAL(resetGLViewer()), ui, SLOT(resetGLViewer()));
69
      if(!ParameterServer::instance()->get<bool>("store_pointclouds") && glviewer != NULL) {
70
          QObject::connect(glviewer, SIGNAL(cloudRendered(pointcloud_type const *)), graph_mgr, SLOT(cloudRendered(pointcloud_type const *))); // 
71
      }
72
    }
73
    QObject::connect(kinect_listener, SIGNAL(setGUIInfo(QString)), ui, SLOT(setInfo(QString)));
74
    QObject::connect(kinect_listener, SIGNAL(setGUIStatus(QString)), ui, SLOT(setStatus(QString)));
75
    QObject::connect(graph_mgr, SIGNAL(setGUIInfo2(QString)), ui, SLOT(setInfo2(QString)));
76
}
77

    
78
int main(int argc, char** argv)
79
{
80
  setlocale(LC_NUMERIC,"C");//Avoid expecting german decimal separators in launch files
81

    
82
  //This can either point to a QCoreApplication (doesn't need an X Server) or an QApplication 
83
  //(which needs Xbut is derived from QCoreApplication)
84
  //Depending an use_gui on the Parameter Server "app" points to an object of the appropriate class
85
  QCoreApplication* app = NULL;
86
  //Similarly this either points to a QMainWindow or a QObject:
87
        QObject* ui = NULL;
88

    
89
  //create thread object, to run the ros event processing loop in parallel to the qt loop
90
  QtROS qtRos(argc, argv, "rgbdslam"); //ros node name & namespace
91
  GLViewer* glviewer = NULL;
92
  bool use_gui = ParameterServer::instance()->get<bool>("use_gui"); 
93
        if (use_gui){
94
      app = new QApplication(argc, argv); //X Server-GUI 
95
      Graphical_UI* gui = new Graphical_UI();
96
      glviewer = gui->getGLViewer();
97
      gui->show();
98
      ui = gui;
99
  } else {
100
      app = new QCoreApplication(argc, argv); //No GUI.  
101
      ui = new RosUi("rgbdslam"); //ui namespace for service calls
102
      ROS_WARN("Running without graphical user interface! See README for how to interact with RGBDSLAM.");
103
  }
104
  //If one thread receives a exit signal from the user, signal the other thread to quit too
105
  QObject::connect(app, SIGNAL(aboutToQuit()), &qtRos, SLOT(quitNow()));
106
  QObject::connect(&qtRos, SIGNAL(rosQuits()), app, SLOT(quit()));
107

    
108
  GraphManager graph_mgr(qtRos.getNodeHandle());
109
  //Instantiate the kinect image listener
110
//  QtConcurrent::run(&kinect_listener, &OpenNIListener::loadBag, bfn);
111
  OpenNIListener kinect_listener(qtRos.getNodeHandle(), &graph_mgr);
112
  QObject::connect(&kinect_listener, SIGNAL(bagFinished()), &qtRos, SLOT(quitNow()));
113
  QObject::connect(&kinect_listener, SIGNAL(bagFinished()), app, SLOT(quit()));
114

    
115

    
116
  ui_connections(ui, &graph_mgr, &kinect_listener);
117
  if (use_gui) gui_connections(ui, &graph_mgr, &kinect_listener, glviewer);
118
  qtRos.start();// Run main loop.
119

    
120
#ifdef USE_ICP_BIN
121
  ROS_INFO("ICP activated via external binary");
122
#endif
123
#ifdef USE_ICP_CODE
124
  ROS_INFO("ICP activated via linked library");
125
#endif
126
  app->exec();
127
  if(ros::ok()) ros::shutdown();//If not yet done through the qt connection
128
  ros::waitForShutdown(); //not sure if necessary. 
129
}
130

    
131