feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,248 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/CameraRGBD.h>
|
||||
#include <rtabmap/core/CameraStereo.h>
|
||||
#include <rtabmap/core/Camera.h>
|
||||
#include <rtabmap/core/SensorEvent.h>
|
||||
#include <rtabmap/core/DBReader.h>
|
||||
#include <rtabmap/core/SensorCaptureThread.h>
|
||||
#include <rtabmap/core/SensorCaptureThread.h>
|
||||
#include <rtabmap/gui/DataRecorder.h>
|
||||
#include <rtabmap/gui/PreferencesDialog.h>
|
||||
#include <QApplication>
|
||||
#include <signal.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
void showUsage()
|
||||
{
|
||||
printf("\nUsage:\n"
|
||||
"dataRecorder [options] config.ini output.db\n"
|
||||
"Description:\n"
|
||||
" A config file contains all camera parameters and driver used. That\n"
|
||||
" file can be generated by RTAB-Map->Preferences->Save Settings(*.ini)\n"
|
||||
" after modifying Source settings.\n"
|
||||
"Options:\n"
|
||||
" -debug Show debug log.\n"
|
||||
" -hide Don't display the current cloud recorded.\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
rtabmap::SensorCaptureThread * cam = 0;
|
||||
QApplication * app = 0;
|
||||
// catch ctrl-c
|
||||
void sighandler(int sig)
|
||||
{
|
||||
printf("\nSignal %d caught...\n", sig);
|
||||
if(cam)
|
||||
{
|
||||
cam->join(true);
|
||||
}
|
||||
if(app)
|
||||
{
|
||||
QMetaObject::invokeMethod(app, "quit");
|
||||
}
|
||||
}
|
||||
|
||||
// Detect when we reached end-of-files
|
||||
class StatusHandler: public UEventsHandler{
|
||||
protected:
|
||||
virtual bool handleEvent(UEvent * event)
|
||||
{
|
||||
if(event->getClassName().compare("SensorEvent") == 0)
|
||||
{
|
||||
SensorEvent * camEvent = (SensorEvent*)event;
|
||||
if(camEvent->getCode() == SensorEvent::kCodeNoMoreImages)
|
||||
{
|
||||
printf("End of stream reached...\n");
|
||||
if(cam)
|
||||
{
|
||||
cam->join(true);
|
||||
}
|
||||
if(app)
|
||||
{
|
||||
QMetaObject::invokeMethod(app, "quit");
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
int main (int argc, char * argv[])
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
|
||||
// parse arguments
|
||||
std::string fileName;
|
||||
bool show = true;
|
||||
std::string configFile;
|
||||
|
||||
if(argc < 3)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
for(int i=1; i<argc-2; ++i)
|
||||
{
|
||||
if(strcmp(argv[i], "-debug") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kDebug);
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-hide") == 0)
|
||||
{
|
||||
show = false;
|
||||
continue;
|
||||
}
|
||||
printf("Unrecognized option : %s\n", argv[i]);
|
||||
showUsage();
|
||||
}
|
||||
configFile = argv[argc-2];
|
||||
configFile = uReplaceChar(configFile, '~', UDirectory::homeDir());
|
||||
fileName = argv[argc-1]; // the last is the output path
|
||||
fileName = uReplaceChar(fileName, '~', UDirectory::homeDir());
|
||||
|
||||
if(UFile::getExtension(fileName).compare("db") != 0)
|
||||
{
|
||||
printf("Database names must end with .db extension\n");
|
||||
showUsage();
|
||||
}
|
||||
|
||||
UINFO("Output = %s", fileName.c_str());
|
||||
UINFO("Show = %s", show?"true":"false");
|
||||
UINFO("Config = %s", configFile.c_str());
|
||||
|
||||
app = new QApplication(argc, argv);
|
||||
|
||||
PreferencesDialog dialog;
|
||||
//Set working directory to default if not in config file to avoid message box
|
||||
ParametersMap paramTmp;
|
||||
Parameters::readINI(configFile, paramTmp);
|
||||
if(paramTmp.find(Parameters::kRtabmapWorkingDirectory()) == paramTmp.end())
|
||||
{
|
||||
paramTmp.clear();
|
||||
paramTmp.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), dialog.getDefaultWorkingDirectory().toStdString()));
|
||||
Parameters::writeINI(configFile, paramTmp);
|
||||
}
|
||||
dialog.init(configFile.c_str());
|
||||
|
||||
UINFO("Driver = %d", dialog.getSourceDriver());
|
||||
UINFO("Rate = %f Hz", dialog.getGeneralInputRate());
|
||||
|
||||
// Catch ctrl-c to close the gui
|
||||
// (Place this after QApplication's constructor)
|
||||
signal(SIGABRT, &sighandler);
|
||||
signal(SIGTERM, &sighandler);
|
||||
signal(SIGINT, &sighandler);
|
||||
|
||||
// Catch end of stream to close the gui
|
||||
StatusHandler statusHandler;
|
||||
statusHandler.registerToEventsManager();
|
||||
|
||||
rtabmap::Camera * camera = dialog.createCamera();
|
||||
if(camera == 0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
ParametersMap parameters = dialog.getAllParameters();
|
||||
cam = new SensorCaptureThread(camera, parameters);
|
||||
cam->setMirroringEnabled(dialog.isSourceMirroring());
|
||||
cam->setColorOnly(dialog.isSourceRGBDColorOnly());
|
||||
cam->setImageDecimation(dialog.getSourceImageDecimation());
|
||||
cam->setHistogramMethod(dialog.getSourceHistogramMethod());
|
||||
cam->setStereoToDepth(dialog.isSourceStereoDepthGenerated());
|
||||
cam->setStereoExposureCompensation(dialog.isSourceStereoExposureCompensation());
|
||||
cam->setScanParameters(
|
||||
dialog.isSourceScanFromDepth(),
|
||||
dialog.getSourceScanDownsampleStep(),
|
||||
dialog.getSourceScanRangeMin(),
|
||||
dialog.getSourceScanRangeMax(),
|
||||
dialog.getSourceScanVoxelSize(),
|
||||
dialog.getSourceScanNormalsK(),
|
||||
dialog.getSourceScanNormalsRadius(),
|
||||
(float)dialog.getSourceScanForceGroundNormalsUp());
|
||||
if(dialog.getIMUFilteringStrategy()>0 && dynamic_cast<DBReader*>(camera) == 0)
|
||||
{
|
||||
cam->enableIMUFiltering(dialog.getIMUFilteringStrategy()-1, parameters, dialog.getIMUFilteringBaseFrameConversion());
|
||||
}
|
||||
if(dialog.isDepthFilteringAvailable())
|
||||
{
|
||||
if(dialog.isBilateralFiltering())
|
||||
{
|
||||
cam->enableBilateralFiltering(
|
||||
dialog.getBilateralSigmaS(),
|
||||
dialog.getBilateralSigmaR());
|
||||
}
|
||||
cam->setDistortionModel(dialog.getSourceDistortionModel().toStdString());
|
||||
}
|
||||
|
||||
DataRecorder recorder;
|
||||
|
||||
if(recorder.init(fileName.c_str()))
|
||||
{
|
||||
recorder.registerToEventsManager();
|
||||
if(show)
|
||||
{
|
||||
recorder.setWindowTitle("Data recorder");
|
||||
recorder.setMinimumWidth(500);
|
||||
recorder.setMinimumHeight(300);
|
||||
recorder.showNormal();
|
||||
app->processEvents();
|
||||
}
|
||||
|
||||
if(camera->init())
|
||||
{
|
||||
cam->start();
|
||||
|
||||
app->exec();
|
||||
|
||||
UINFO("Closing...");
|
||||
|
||||
recorder.close();
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Cannot initialize the camera!");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Cannot initialize the recorder! Maybe the path is wrong: \"%s\"", fileName.c_str());
|
||||
}
|
||||
|
||||
if(cam)
|
||||
{
|
||||
delete cam;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user