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