main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <rtabmap/core/OdometryF2M.h>
00029 #include "rtabmap/core/Rtabmap.h"
00030 #include "rtabmap/core/RtabmapThread.h"
00031 #include "rtabmap/core/CameraRGBD.h"
00032 #include "rtabmap/core/CameraThread.h"
00033 #include "rtabmap/core/OdometryThread.h"
00034 #include "rtabmap/utilite/UEventsManager.h"
00035 #include <QApplication>
00036 #include <stdio.h>
00037 
00038 #include "MapBuilderWifi.h"
00039 
00040 #include "WifiThread.h"
00041 
00042 void showUsage()
00043 {
00044         printf("\nUsage:\n"
00045                         "rtabmap-wifi_mapping [options]\n"
00046                         "Options:\n"
00047                         "  -i \"name\"            Wifi interface name (e.g. \"eth0\"). Only required on Linux.\n"
00048                         "  -m                     Enable mirroring of the camera image.\n"
00049                         "  -d #                   Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
00050         exit(1);
00051 }
00052 
00053 using namespace rtabmap;
00054 int main(int argc, char * argv[])
00055 {
00056         ULogger::setType(ULogger::kTypeConsole);
00057         ULogger::setLevel(ULogger::kWarning);
00058 
00059         std::string interfaceName = "wlan0";
00060         int driver = 0;
00061         bool mirroring = false;
00062 
00063         // parse options
00064         for(int i = 1; i<argc; ++i)
00065         {
00066                 if(strcmp(argv[i], "-i") == 0)
00067                 {
00068                         ++i;
00069                         if(i < argc)
00070                         {
00071                                 interfaceName = argv[i];
00072                         }
00073                         else
00074                         {
00075                                 showUsage();
00076                         }
00077                         continue;
00078                 }
00079                 if(strcmp(argv[i], "-m") == 0)
00080                 {
00081                         mirroring = true;
00082                         continue;
00083                 }
00084                 if(strcmp(argv[i], "-d") == 0)
00085                 {
00086                         ++i;
00087                         if(i < argc)
00088                         {
00089                                 driver = atoi(argv[i]);
00090                                 if(driver < 0 || driver > 4)
00091                                 {
00092                                         UERROR("driver should be between 0 and 4.");
00093                                         showUsage();
00094                                 }
00095                         }
00096                         else
00097                         {
00098                                 showUsage();
00099                         }
00100                         continue;
00101                 }
00102 
00103                 UERROR("Option \"%s\" not recognized!", argv[i]);
00104                 showUsage();
00105         }
00106 
00107         // Here is the pipeline that we will use:
00108         // CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
00109 
00110         // Create the OpenNI camera, it will send a CameraEvent at the rate specified.
00111         // Set transform to camera so z is up, y is left and x going forward
00112         Camera * camera = 0;
00113         Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
00114         if(driver == 1)
00115         {
00116                 if(!CameraOpenNI2::available())
00117                 {
00118                         UERROR("Not built with OpenNI2 support...");
00119                         exit(-1);
00120                 }
00121                 camera = new CameraOpenNI2("", CameraOpenNI2::kTypeColorDepth, 0, opticalRotation);
00122         }
00123         else if(driver == 2)
00124         {
00125                 if(!CameraFreenect::available())
00126                 {
00127                         UERROR("Not built with Freenect support...");
00128                         exit(-1);
00129                 }
00130                 camera = new CameraFreenect(0, CameraFreenect::kTypeColorDepth, 0, opticalRotation);
00131         }
00132         else if(driver == 3)
00133         {
00134                 if(!CameraOpenNICV::available())
00135                 {
00136                         UERROR("Not built with OpenNI from OpenCV support...");
00137                         exit(-1);
00138                 }
00139                 camera = new CameraOpenNICV(false, 0, opticalRotation);
00140         }
00141         else if(driver == 4)
00142         {
00143                 if(!CameraOpenNICV::available())
00144                 {
00145                         UERROR("Not built with OpenNI from OpenCV support...");
00146                         exit(-1);
00147                 }
00148                 camera = new CameraOpenNICV(true, 0, opticalRotation);
00149         }
00150         else
00151         {
00152                 camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
00153         }
00154 
00155 
00156         if(!camera->init())
00157         {
00158                 UERROR("Camera init failed! Try another camera driver.");
00159                 showUsage();
00160                 exit(1);
00161         }
00162         CameraThread cameraThread(camera);
00163         if(mirroring)
00164         {
00165                 cameraThread.setMirroringEnabled(true);
00166         }
00167 
00168         // GUI stuff, there the handler will receive RtabmapEvent and construct the map
00169         // We give it the camera so the GUI can pause/resume the camera
00170         QApplication app(argc, argv);
00171         MapBuilderWifi mapBuilderWifi(&cameraThread);
00172 
00173         // Create an odometry thread to process camera events, it will send OdometryEvent.
00174         OdometryThread odomThread(new OdometryF2M());
00175 
00176         // Create RTAB-Map to process OdometryEvent
00177         Rtabmap * rtabmap = new Rtabmap();
00178         ParametersMap param;
00179         param.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // disable rehearsal (node merging when not moving)
00180         rtabmap->init(param);
00181         RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
00182 
00183         // Create Wifi monitoring thread
00184         WifiThread wifiThread(interfaceName); // 0.5 Hz, should be under RTAB-Map rate (which is 1 Hz by default)
00185 
00186         // Setup handlers
00187         odomThread.registerToEventsManager();
00188         rtabmapThread.registerToEventsManager();
00189         mapBuilderWifi.registerToEventsManager();
00190 
00191         // The RTAB-Map is subscribed by default to CameraEvent, but we want
00192         // RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent.
00193         // We can do that by creating a "pipe" between the camera and odometry, then
00194         // only the odometry will receive CameraEvent from that camera. RTAB-Map is
00195         // also subscribed to OdometryEvent by default, so no need to create a pipe between
00196         // odometry and RTAB-Map.
00197         UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
00198 
00199         // Let's start the threads
00200         rtabmapThread.start();
00201         odomThread.start();
00202         cameraThread.start();
00203         wifiThread.start();
00204 
00205         mapBuilderWifi.show();
00206         app.exec(); // main loop
00207 
00208         // remove handlers
00209         mapBuilderWifi.unregisterFromEventsManager();
00210         rtabmapThread.unregisterFromEventsManager();
00211         odomThread.unregisterFromEventsManager();
00212 
00213         // Kill all threads
00214         cameraThread.kill();
00215         odomThread.join(true);
00216         rtabmapThread.join(true);
00217         wifiThread.join(true);
00218 
00219         return 0;
00220 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20