examples/WifiMapping/main.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/Odometry.h>
30 #include "rtabmap/core/Rtabmap.h"
36 #include <QApplication>
37 #include <stdio.h>
38 
39 #ifdef RTABMAP_PYTHON
41 #endif
42 
43 #include "MapBuilderWifi.h"
44 
45 #include "WifiThread.h"
46 
47 void showUsage()
48 {
49  printf("\nUsage:\n"
50  "rtabmap-wifi_mapping [options]\n"
51  "Options:\n"
52  " -i \"name\" Wifi interface name (e.g. \"eth0\"). Only required on Linux.\n"
53  " -m Enable mirroring of the camera image.\n"
54  " -d # Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2 9=Kinect for Azure SDK 10=MYNT EYE S\n\n");
55  exit(1);
56 }
57 
58 using namespace rtabmap;
59 int main(int argc, char * argv[])
60 {
63 
64 #ifdef RTABMAP_PYTHON
65  PythonInterface python; // Make sure we initialize python in main thread
66 #endif
67 
68  std::string interfaceName = "wlan0";
69  int driver = 0;
70  bool mirroring = false;
71 
72  // parse options
73  for(int i = 1; i<argc; ++i)
74  {
75  if(strcmp(argv[i], "-i") == 0)
76  {
77  ++i;
78  if(i < argc)
79  {
80  interfaceName = argv[i];
81  }
82  else
83  {
84  showUsage();
85  }
86  continue;
87  }
88  if(strcmp(argv[i], "-m") == 0)
89  {
90  mirroring = true;
91  continue;
92  }
93  if(strcmp(argv[i], "-d") == 0)
94  {
95  ++i;
96  if(i < argc)
97  {
98  driver = atoi(argv[i]);
99  if(driver < 0 || driver > 8)
100  {
101  UERROR("driver should be between 0 and 8.");
102  showUsage();
103  }
104  }
105  else
106  {
107  showUsage();
108  }
109  continue;
110  }
111 
112  UERROR("Option \"%s\" not recognized!", argv[i]);
113  showUsage();
114  }
115 
116  // Here is the pipeline that we will use:
117  // CameraOpenni -> "" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
118 
119  // Create the OpenNI camera, it will send a at the rate specified.
120  // Set transform to camera so z is up, y is left and x going forward
121  Camera * camera = 0;
122  Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
123  if(driver == 1)
124  {
126  {
127  UERROR("Not built with OpenNI2 support...");
128  exit(-1);
129  }
130  camera = new CameraOpenNI2("", CameraOpenNI2::kTypeColorDepth, 0, opticalRotation);
131  }
132  else if(driver == 2)
133  {
135  {
136  UERROR("Not built with Freenect support...");
137  exit(-1);
138  }
139  camera = new CameraFreenect(0, CameraFreenect::kTypeColorDepth, 0, opticalRotation);
140  }
141  else if(driver == 3)
142  {
144  {
145  UERROR("Not built with OpenNI from OpenCV support...");
146  exit(-1);
147  }
148  camera = new CameraOpenNICV(false, 0, opticalRotation);
149  }
150  else if(driver == 4)
151  {
153  {
154  UERROR("Not built with OpenNI from OpenCV support...");
155  exit(-1);
156  }
157  camera = new CameraOpenNICV(true, 0, opticalRotation);
158  }
159  else if (driver == 5)
160  {
162  {
163  UERROR("Not built with Freenect2 support...");
164  exit(-1);
165  }
166  camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD, 0, opticalRotation);
167  }
168  else if (driver == 6)
169  {
171  {
172  UERROR("Not built with ZED SDK support...");
173  exit(-1);
174  }
175  camera = new CameraStereoZed(0, 2, 1, 1, 100, false, 0, opticalRotation);
176  }
177  else if (driver == 7)
178  {
180  {
181  UERROR("Not built with RealSense support...");
182  exit(-1);
183  }
184  camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
185  }
186  else if (driver == 8)
187  {
189  {
190  UERROR("Not built with RealSense2 support...");
191  exit(-1);
192  }
193  camera = new CameraRealSense2("", 0, opticalRotation);
194  }
195  else if (driver == 9)
196  {
198  {
199  UERROR("Not built with Kinect for Azure SDK support...");
200  exit(-1);
201  }
202  camera = new rtabmap::CameraK4A(1);
203  }
204  else if (driver == 10)
205  {
207  {
208  UERROR("Not built with Mynt Eye S support...");
209  exit(-1);
210  }
212  }
213  else
214  {
215  camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
216  }
217 
218 
219  if(!camera->init())
220  {
221  UERROR("Camera init failed! Try another camera driver.");
222  showUsage();
223  exit(1);
224  }
225  SensorCaptureThread cameraThread(camera);
226  if(mirroring)
227  {
228  cameraThread.setMirroringEnabled(true);
229  }
230 
231  // GUI stuff, there the handler will receive RtabmapEvent and construct the map
232  // We give it the camera so the GUI can pause/resume the camera
233  QApplication app(argc, argv);
234  MapBuilderWifi mapBuilderWifi(&cameraThread);
235 
236  // Create an odometry thread to process camera events, it will send OdometryEvent.
237  OdometryThread odomThread(Odometry::create());
238 
239  // Create RTAB-Map to process OdometryEvent
240  Rtabmap * rtabmap = new Rtabmap();
241  ParametersMap param;
242  param.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // disable rehearsal (node merging when not moving)
243  param.insert(ParametersPair(Parameters::kRGBDLinearUpdate(), "0")); // disable node ignored when not moving
244  param.insert(ParametersPair(Parameters::kRGBDAngularUpdate(), "0")); // disable node ignored when not moving
245  rtabmap->init(param);
246  RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
247 
248  // Create Wifi monitoring thread
249  WifiThread wifiThread(interfaceName); // 0.5 Hz, should be under RTAB-Map rate (which is 1 Hz by default)
250 
251  // Setup handlers
252  odomThread.registerToEventsManager();
253  rtabmapThread.registerToEventsManager();
254  mapBuilderWifi.registerToEventsManager();
255 
256  // The RTAB-Map is subscribed by default to , but we want
257  // RTAB-Map to process OdometryEvent instead, ignoring the .
258  // We can do that by creating a "pipe" between the camera and odometry, then
259  // only the odometry will receive from that camera. RTAB-Map is
260  // also subscribed to OdometryEvent by default, so no need to create a pipe between
261  // odometry and RTAB-Map.
262  UEventsManager::createPipe(&cameraThread, &odomThread, "");
263 
264  // Let's start the threads
265  rtabmapThread.start();
266  odomThread.start();
267  cameraThread.start();
268  wifiThread.start();
269 
270  mapBuilderWifi.show();
271  app.exec(); // main loop
272 
273  // remove handlers
274  mapBuilderWifi.unregisterFromEventsManager();
275  rtabmapThread.unregisterFromEventsManager();
276  odomThread.unregisterFromEventsManager();
277 
278  // Kill all threads
279  cameraThread.kill();
280  odomThread.join(true);
281  rtabmapThread.join(true);
282  wifiThread.join(true);
283 
284  return 0;
285 }
rtabmap::SensorCaptureThread
Definition: SensorCaptureThread.h:58
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
MapBuilderWifi.h
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:58
SensorCaptureThread.h
MapBuilderWifi
Definition: MapBuilderWifi.h:54
rtabmap::OdometryThread
Definition: OdometryThread.h:41
rtabmap::CameraFreenect::available
static bool available()
Definition: CameraFreenect.cpp:284
UEventsHandler::registerToEventsManager
void registerToEventsManager()
Definition: UEventsHandler.cpp:29
rtabmap::CameraRealSense::available
static bool available()
Definition: CameraRealSense.cpp:47
OdometryThread.h
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
WifiThread.h
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
WifiThread
Definition: WifiThread.h:77
rtabmap::CameraMyntEye
Definition: CameraMyntEye.h:47
CameraRGBD.h
CameraStereo.h
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
RtabmapThread.h
rtabmap::CameraFreenect2::available
static bool available()
Definition: CameraFreenect2.cpp:45
rtabmap::CameraK4A
Definition: CameraK4A.h:43
rtabmap::CameraFreenect
Definition: CameraFreenect.h:44
Odometry.h
rtabmap::SensorCapture::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
app
QApplication * app
Definition: tools/DataRecorder/main.cpp:59
rtabmap::CameraFreenect2
Definition: CameraFreenect2.h:46
rtabmap::CameraOpenNI2
Definition: CameraOpenNI2.h:42
rtabmap::CameraRealSense2
Definition: CameraRealSense2.h:54
rtabmap::CameraFreenect2::kTypeColor2DepthSD
@ kTypeColor2DepthSD
Definition: CameraFreenect2.h:53
UEventsManager::createPipe
static void createPipe(const UEventsSender *sender, const UEventsHandler *receiver, const std::string &eventName)
Definition: UEventsManager.cpp:67
rtabmap::PythonInterface
Definition: PythonInterface.h:26
Rtabmap.h
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
python
UThread::kill
void kill()
Definition: UThread.cpp:48
UEventsHandler::unregisterFromEventsManager
void unregisterFromEventsManager()
Definition: UEventsHandler.cpp:33
main
int main(int argc, char *argv[])
Definition: examples/WifiMapping/main.cpp:59
rtabmap::SensorCaptureThread::setMirroringEnabled
void setMirroringEnabled(bool enabled)
Definition: SensorCaptureThread.h:123
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
UThread::start
void start()
Definition: UThread.cpp:122
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
PythonInterface.h
rtabmap::CameraOpenNI2::kTypeColorDepth
@ kTypeColorDepth
Definition: CameraOpenNI2.h:49
rtabmap::CameraStereoZed::available
static bool available()
Definition: CameraStereoZed.cpp:235
rtabmap::Camera
Definition: Camera.h:43
rtabmap::CameraOpenNICV
Definition: CameraOpenNICV.h:36
rtabmap::CameraStereoZed
Definition: CameraStereoZed.h:43
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CameraOpenNICV::available
static bool available()
Definition: CameraOpenNICV.cpp:36
rtabmap::CameraMyntEye::available
static bool available()
Definition: CameraMyntEye.cpp:506
rtabmap::CameraRealSense2::available
static bool available()
Definition: CameraRealSense2.cpp:45
rtabmap::CameraK4A::available
static bool available()
Definition: CameraK4A.cpp:42
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
rtabmap::CameraRealSense
Definition: CameraRealSense.h:49
rtabmap::CameraOpenNI2::available
static bool available()
Definition: CameraOpenNI2.cpp:42
rtabmap::Rtabmap
Definition: Rtabmap.h:54
rtabmap::CameraFreenect::kTypeColorDepth
@ kTypeColorDepth
Definition: CameraFreenect.h:49
rtabmap
Definition: CameraARCore.cpp:35
UEventsManager.h
UERROR
#define UERROR(...)
showUsage
void showUsage()
Definition: examples/WifiMapping/main.cpp:47
i
int i
rtabmap::CameraOpenni
Definition: CameraOpenni.h:55
rtabmap::RtabmapThread
Definition: RtabmapThread.h:51


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11