00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/Rtabmap.h"
00029 #include "rtabmap/core/RtabmapThread.h"
00030 #include "rtabmap/core/RtabmapEvent.h"
00031 #include "rtabmap/core/Camera.h"
00032 #include "rtabmap/core/CameraEvent.h"
00033 #include "rtabmap/core/ParamEvent.h"
00034 #include "rtabmap/core/OdometryEvent.h"
00035 #include "rtabmap/core/UserDataEvent.h"
00036 #include "rtabmap/core/Memory.h"
00037
00038 #include <rtabmap/utilite/ULogger.h>
00039 #include <rtabmap/utilite/UEventsManager.h>
00040 #include <rtabmap/utilite/UStl.h>
00041 #include <rtabmap/utilite/UTimer.h>
00042 #include <rtabmap/utilite/UConversion.h>
00043
00044 namespace rtabmap {
00045
00046 RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
00047 _dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
00048 _rate(Parameters::defaultRtabmapDetectionRate()),
00049 _createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
00050 _frameRateTimer(new UTimer()),
00051 _previousStamp(0.0),
00052 _rtabmap(rtabmap),
00053 _paused(false),
00054 lastPose_(Transform::getIdentity())
00055
00056 {
00057 UASSERT(rtabmap != 0);
00058 }
00059
00060 RtabmapThread::~RtabmapThread()
00061 {
00062 UEventsManager::removeHandler(this);
00063
00064 close(true);
00065
00066 delete _frameRateTimer;
00067 }
00068
00069 void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
00070 {
00071 ULOGGER_DEBUG("to %d", newState);
00072
00073 _stateMutex.lock();
00074 {
00075 _state.push(newState);
00076 _stateParam.push(parameters);
00077 }
00078 _stateMutex.unlock();
00079
00080 _dataAdded.release();
00081 }
00082
00083 void RtabmapThread::clearBufferedData()
00084 {
00085 _dataMutex.lock();
00086 {
00087 _dataBuffer.clear();
00088 _newMapEvents.clear();
00089 lastPose_.setIdentity();
00090 covariance_ = cv::Mat();
00091 _previousStamp = 0;
00092 }
00093 _dataMutex.unlock();
00094
00095 _userDataMutex.lock();
00096 {
00097 _userData = cv::Mat();
00098 }
00099 _userDataMutex.unlock();
00100 }
00101
00102 void RtabmapThread::setDetectorRate(float rate)
00103 {
00104 UASSERT(rate >= 0.0f);
00105 _rate = rate;
00106 }
00107
00108 void RtabmapThread::setDataBufferSize(unsigned int size)
00109 {
00110 _dataBufferMaxSize = size;
00111 }
00112
00113 void RtabmapThread::createIntermediateNodes(bool enabled)
00114 {
00115 _createIntermediateNodes = enabled;
00116 }
00117
00118 void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabasePath)
00119 {
00120 this->join(true);
00121 if(_rtabmap)
00122 {
00123 _rtabmap->close(databaseSaved, ouputDatabasePath);
00124 delete _rtabmap;
00125 _rtabmap = 0;
00126 }
00127 }
00128
00129 void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
00130 {
00131 if(_rtabmap)
00132 {
00133 std::map<int, Signature> signatures;
00134 std::map<int, Transform> poses;
00135 std::multimap<int, Link> constraints;
00136 std::map<int, int> mapIds;
00137 std::map<int, double> stamps;
00138 std::map<int, std::string> labels;
00139 std::map<int, std::vector<unsigned char> > userDatas;
00140
00141 if(graphOnly)
00142 {
00143 _rtabmap->getGraph(poses,
00144 constraints,
00145 optimized,
00146 full,
00147 &signatures);
00148 }
00149 else
00150 {
00151 _rtabmap->get3DMap(
00152 signatures,
00153 poses,
00154 constraints,
00155 optimized,
00156 full);
00157 }
00158
00159 this->post(new RtabmapEvent3DMap(
00160 signatures,
00161 poses,
00162 constraints));
00163 }
00164 else
00165 {
00166 UERROR("Rtabmap is null!");
00167 }
00168 }
00169
00170 void RtabmapThread::mainLoopBegin()
00171 {
00172 ULogger::registerCurrentThread("Rtabmap");
00173 if(_rtabmap == 0)
00174 {
00175 UERROR("Cannot start rtabmap thread if no rtabmap object is set! Stopping the thread...");
00176 this->kill();
00177 }
00178 }
00179
00180 void RtabmapThread::mainLoopKill()
00181 {
00182 this->clearBufferedData();
00183
00184 _dataAdded.release();
00185 }
00186
00187 void RtabmapThread::mainLoop()
00188 {
00189 State state = kStateDetecting;
00190 ParametersMap parameters;
00191
00192 _stateMutex.lock();
00193 {
00194 if(!_state.empty() && !_stateParam.empty())
00195 {
00196 state = _state.front();
00197 _state.pop();
00198 parameters = _stateParam.front();
00199 _stateParam.pop();
00200 }
00201 }
00202 _stateMutex.unlock();
00203
00204 int id = 0;
00205 cv::Mat userData;
00206 UTimer timer;
00207 std::string str;
00208 switch(state)
00209 {
00210 case kStateDetecting:
00211 this->process();
00212 break;
00213 case kStateInit:
00214 UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
00215 str = parameters.at("RtabmapThread/DatabasePath");
00216 parameters.erase("RtabmapThread/DatabasePath");
00217 Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
00218 Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
00219 Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
00220 UASSERT(_dataBufferMaxSize >= 0);
00221 UASSERT(_rate >= 0.0f);
00222 _rtabmap->init(parameters, str);
00223 break;
00224 case kStateChangingParameters:
00225 Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
00226 Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
00227 Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
00228 UASSERT(_dataBufferMaxSize >= 0);
00229 UASSERT(_rate >= 0.0f);
00230 _rtabmap->parseParameters(parameters);
00231 break;
00232 case kStateReseting:
00233 _rtabmap->resetMemory();
00234 this->clearBufferedData();
00235 break;
00236 case kStateClose:
00237 if(_dataBuffer.size())
00238 {
00239 UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
00240 this->clearBufferedData();
00241 }
00242 _rtabmap->close(uStr2Bool(parameters.at("saved")), parameters.at("outputPath"));
00243 break;
00244 case kStateDumpingMemory:
00245 _rtabmap->dumpData();
00246 break;
00247 case kStateDumpingPrediction:
00248 _rtabmap->dumpPrediction();
00249 break;
00250 case kStateExportingDOTGraph:
00251 _rtabmap->generateDOTGraph(
00252 parameters.at("path"),
00253 atoi(parameters.at("id").c_str()),
00254 atoi(parameters.at("margin").c_str()));
00255 break;
00256 case kStateExportingPoses:
00257 _rtabmap->exportPoses(
00258 parameters.at("path"),
00259 uStr2Bool(parameters.at("optimized")),
00260 uStr2Bool(parameters.at("global")),
00261 atoi(parameters.at("type").c_str()));
00262 break;
00263 case kStateCleanDataBuffer:
00264 this->clearBufferedData();
00265 break;
00266 case kStatePublishingMap:
00267 this->publishMap(
00268 uStr2Bool(parameters.at("optimized")),
00269 uStr2Bool(parameters.at("global")),
00270 uStr2Bool(parameters.at("graph_only")));
00271 break;
00272 case kStateTriggeringMap:
00273 _rtabmap->triggerNewMap();
00274 break;
00275 case kStateSettingGoal:
00276 id = atoi(parameters.at("id").c_str());
00277 if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
00278 {
00279 id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
00280 if(id <= 0)
00281 {
00282 UERROR("Failed to find a node with label \"%s\".", parameters.at("label").c_str());
00283 }
00284 }
00285 else if(id < 0)
00286 {
00287 UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
00288 }
00289 timer.start();
00290 if(id > 0 && !_rtabmap->computePath(id, true))
00291 {
00292 UERROR("Failed to compute a path to goal %d.", id);
00293 }
00294 this->post(new RtabmapGlobalPathEvent(
00295 id,
00296 parameters.at("label"),
00297 _rtabmap->getPath(),
00298 timer.elapsed()));
00299 break;
00300 case kStateCancellingGoal:
00301 _rtabmap->clearPath(0);
00302 break;
00303 case kStateLabelling:
00304 if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()))
00305 {
00306 this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()));
00307 }
00308 break;
00309 default:
00310 UFATAL("Invalid state !?!?");
00311 break;
00312 }
00313 }
00314
00315
00316 bool RtabmapThread::handleEvent(UEvent* event)
00317 {
00318 if(this->isRunning())
00319 {
00320 if(event->getClassName().compare("CameraEvent") == 0)
00321 {
00322 UDEBUG("CameraEvent");
00323 CameraEvent * e = (CameraEvent*)event;
00324 if(e->getCode() == CameraEvent::kCodeData)
00325 {
00326 if (_rtabmap->isRGBDMode())
00327 {
00328 if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
00329 {
00330 OdometryInfo infoCov;
00331 infoCov.reg.covariance = e->info().odomCovariance;
00332 if(e->info().odomVelocity.size() == 6)
00333 {
00334 infoCov.transform = Transform(
00335 e->info().odomVelocity[0],
00336 e->info().odomVelocity[1],
00337 e->info().odomVelocity[2],
00338 e->info().odomVelocity[3],
00339 e->info().odomVelocity[4],
00340 e->info().odomVelocity[5]);
00341 infoCov.interval = 1.0;
00342 }
00343 this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
00344 }
00345 else
00346 {
00347 lastPose_.setNull();
00348 }
00349 }
00350 else
00351 {
00352 OdometryInfo infoCov;
00353 infoCov.reg.covariance = e->info().odomCovariance;
00354 this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
00355 }
00356
00357 }
00358 }
00359 else if(event->getClassName().compare("OdometryEvent") == 0)
00360 {
00361 UDEBUG("OdometryEvent");
00362 OdometryEvent * e = (OdometryEvent*)event;
00363 if(!e->pose().isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
00364 {
00365 this->addData(*e);
00366 }
00367 else
00368 {
00369 lastPose_.setNull();
00370 }
00371 }
00372 else if(event->getClassName().compare("UserDataEvent") == 0)
00373 {
00374 if(!_paused)
00375 {
00376 UDEBUG("UserDataEvent");
00377 bool updated = false;
00378 UserDataEvent * e = (UserDataEvent*)event;
00379 _userDataMutex.lock();
00380 if(!e->data().empty())
00381 {
00382 updated = !_userData.empty();
00383 _userData = e->data();
00384 }
00385 _userDataMutex.unlock();
00386 if(updated)
00387 {
00388 UWARN("New user data received before the last one was processed... replacing "
00389 "user data with this new one. Note that UserDataEvent should be used only "
00390 "if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
00391 }
00392 }
00393 }
00394 else if(event->getClassName().compare("RtabmapEventCmd") == 0)
00395 {
00396 RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
00397 RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
00398 if(cmd == RtabmapEventCmd::kCmdInit)
00399 {
00400 ULOGGER_DEBUG("CMD_INIT");
00401 ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
00402 UASSERT(rtabmapEvent->value1().isStr());
00403 UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
00404 pushNewState(kStateInit, parameters);
00405 }
00406 else if(cmd == RtabmapEventCmd::kCmdClose)
00407 {
00408 ULOGGER_DEBUG("CMD_CLOSE");
00409 UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
00410 ParametersMap param;
00411 param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
00412 param.insert(ParametersPair("outputPath", rtabmapEvent->value2().toStr()));
00413 pushNewState(kStateClose, param);
00414 }
00415 else if(cmd == RtabmapEventCmd::kCmdResetMemory)
00416 {
00417 ULOGGER_DEBUG("CMD_RESET_MEMORY");
00418 pushNewState(kStateReseting);
00419 }
00420 else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
00421 {
00422 ULOGGER_DEBUG("CMD_DUMP_MEMORY");
00423 pushNewState(kStateDumpingMemory);
00424 }
00425 else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
00426 {
00427 ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
00428 pushNewState(kStateDumpingPrediction);
00429 }
00430 else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
00431 {
00432 ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
00433 UASSERT(rtabmapEvent->value1().isBool());
00434 UASSERT(rtabmapEvent->value2().isStr());
00435 UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
00436 UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
00437 ParametersMap param;
00438 param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
00439 param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
00440 param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
00441 pushNewState(kStateExportingDOTGraph, param);
00442 }
00443 else if(cmd == RtabmapEventCmd::kCmdExportPoses)
00444 {
00445 ULOGGER_DEBUG("CMD_EXPORT_POSES");
00446 UASSERT(rtabmapEvent->value1().isBool());
00447 UASSERT(rtabmapEvent->value2().isBool());
00448 UASSERT(rtabmapEvent->value3().isStr());
00449 UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
00450 ParametersMap param;
00451 param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
00452 param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
00453 param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
00454 param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
00455 pushNewState(kStateExportingPoses, param);
00456
00457 }
00458 else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
00459 {
00460 ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
00461 pushNewState(kStateCleanDataBuffer);
00462 }
00463 else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
00464 {
00465 ULOGGER_DEBUG("CMD_PUBLISH_MAP");
00466 UASSERT(rtabmapEvent->value1().isBool());
00467 UASSERT(rtabmapEvent->value2().isBool());
00468 UASSERT(rtabmapEvent->value3().isBool());
00469 ParametersMap param;
00470 param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
00471 param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
00472 param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
00473 pushNewState(kStatePublishingMap, param);
00474 }
00475 else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
00476 {
00477 ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
00478 pushNewState(kStateTriggeringMap);
00479 }
00480 else if(cmd == RtabmapEventCmd::kCmdPause)
00481 {
00482 ULOGGER_DEBUG("CMD_PAUSE");
00483 _paused = !_paused;
00484 }
00485 else if(cmd == RtabmapEventCmd::kCmdGoal)
00486 {
00487 ULOGGER_DEBUG("CMD_GOAL");
00488 UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
00489 ParametersMap param;
00490 param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
00491 param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
00492 pushNewState(kStateSettingGoal, param);
00493 }
00494 else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
00495 {
00496 ULOGGER_DEBUG("CMD_CANCEL_GOAL");
00497 pushNewState(kStateCancellingGoal);
00498 }
00499 else if(cmd == RtabmapEventCmd::kCmdLabel)
00500 {
00501 ULOGGER_DEBUG("CMD_LABEL");
00502 UASSERT(rtabmapEvent->value1().isStr());
00503 UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
00504 ParametersMap param;
00505 param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
00506 param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
00507 pushNewState(kStateLabelling, param);
00508 }
00509 else
00510 {
00511 UWARN("Cmd %d unknown!", cmd);
00512 }
00513 }
00514 else if(event->getClassName().compare("ParamEvent") == 0)
00515 {
00516 ULOGGER_DEBUG("changing parameters");
00517 pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
00518 }
00519 }
00520 return false;
00521 }
00522
00523
00524
00525
00526 void RtabmapThread::process()
00527 {
00528 OdometryEvent data;
00529 if(_state.empty() && getData(data))
00530 {
00531 if(_rtabmap->getMemory())
00532 {
00533 bool wasPlanning = _rtabmap->getPath().size()>0;
00534 if(_rtabmap->process(data.data(), data.pose(), data.covariance(), data.velocity()))
00535 {
00536 Statistics stats = _rtabmap->getStatistics();
00537 stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
00538 ULOGGER_DEBUG("posting statistics_ event...");
00539 this->post(new RtabmapEvent(stats));
00540
00541 if(wasPlanning && _rtabmap->getPath().size() == 0)
00542 {
00543
00544 this->post(new RtabmapGoalStatusEvent(_rtabmap->getPathStatus()));
00545 }
00546 }
00547 }
00548 else
00549 {
00550 UERROR("RTAB-Map is not initialized! Ignoring received data...");
00551 }
00552 }
00553 }
00554
00555 void RtabmapThread::addData(const OdometryEvent & odomEvent)
00556 {
00557 if(!_paused)
00558 {
00559 UScopeMutex scopeMutex(_dataMutex);
00560
00561 bool ignoreFrame = false;
00562 if(_rate>0.0f)
00563 {
00564 if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
00565 ((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
00566 {
00567 ignoreFrame = true;
00568 }
00569 }
00570 if(!lastPose_.isIdentity() &&
00571 (odomEvent.pose().isIdentity() ||
00572 odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
00573 {
00574 if(odomEvent.pose().isIdentity())
00575 {
00576 UWARN("Odometry is reset (identity pose detected). Increment map id! (stamp=%fs)", odomEvent.data().stamp());
00577 }
00578 else
00579 {
00580 UWARN("Odometry is reset (high variance (%f >=9999 detected, stamp=%fs). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0), odomEvent.data().stamp());
00581 }
00582 _newMapEvents.push_back(odomEvent.data().stamp());
00583 covariance_ = cv::Mat();
00584 }
00585
00586 if(uIsFinite(odomEvent.info().reg.covariance.at<double>(0,0)) &&
00587 odomEvent.info().reg.covariance.at<double>(0,0) != 1.0 &&
00588 odomEvent.info().reg.covariance.at<double>(0,0)>0.0)
00589 {
00590
00591 if(covariance_.empty() || odomEvent.info().reg.covariance.at<double>(0,0) > covariance_.at<double>(0,0))
00592 {
00593 covariance_ = odomEvent.info().reg.covariance;
00594 }
00595 }
00596
00597 if(ignoreFrame && !_createIntermediateNodes)
00598 {
00599 return;
00600 }
00601 else if(!ignoreFrame)
00602 {
00603 _frameRateTimer->start();
00604 _previousStamp = odomEvent.data().stamp();
00605 }
00606
00607 lastPose_ = odomEvent.pose();
00608
00609 bool notify = true;
00610
00611 if(covariance_.empty())
00612 {
00613 covariance_ = cv::Mat::eye(6,6,CV_64FC1);
00614 }
00615 OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
00616 odomInfo.reg.covariance = covariance_;
00617 if(ignoreFrame)
00618 {
00619
00620 SensorData tmp = odomEvent.data();
00621 tmp.setId(-1);
00622 tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
00623 _dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
00624 }
00625 else
00626 {
00627 _dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), odomInfo));
00628 }
00629 UINFO("Added data %d", odomEvent.data().id());
00630
00631 covariance_ = cv::Mat();
00632 while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
00633 {
00634 if(_rate > 0.0f)
00635 {
00636 ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
00637 }
00638 _dataBuffer.pop_front();
00639 notify = false;
00640 }
00641
00642 if(notify)
00643 {
00644 _dataAdded.release();
00645 }
00646 }
00647 }
00648
00649 bool RtabmapThread::getData(OdometryEvent & data)
00650 {
00651 ULOGGER_DEBUG("");
00652
00653 ULOGGER_INFO("waiting for data");
00654 _dataAdded.acquire();
00655 ULOGGER_INFO("wake-up");
00656
00657 bool dataFilled = false;
00658 bool triggerNewMap = false;
00659 _dataMutex.lock();
00660 {
00661 if(_state.empty() && !_dataBuffer.empty())
00662 {
00663 data = _dataBuffer.front();
00664 _dataBuffer.pop_front();
00665
00666 _userDataMutex.lock();
00667 {
00668 if(!_userData.empty())
00669 {
00670 data.data().setUserData(_userData);
00671 _userData = cv::Mat();
00672 }
00673 }
00674 _userDataMutex.unlock();
00675
00676 while(_newMapEvents.size() && _newMapEvents.front() <= data.data().stamp())
00677 {
00678 UWARN("Triggering new map %f<=%f...", _newMapEvents.front() , data.data().stamp());
00679 triggerNewMap = true;
00680 _newMapEvents.pop_front();
00681 }
00682
00683 dataFilled = true;
00684 }
00685 }
00686 _dataMutex.unlock();
00687
00688 if(triggerNewMap)
00689 {
00690 _rtabmap->triggerNewMap();
00691 }
00692
00693 return dataFilled;
00694 }
00695
00696 }