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 _frameRateTimer(new UTimer()),
00050 _rtabmap(rtabmap),
00051 _paused(false),
00052 lastPose_(Transform::getIdentity()),
00053 _rotVariance(0),
00054 _transVariance(0)
00055
00056 {
00057 UASSERT(rtabmap != 0);
00058 }
00059
00060 RtabmapThread::~RtabmapThread()
00061 {
00062 UEventsManager::removeHandler(this);
00063
00064
00065 join(true);
00066
00067 delete _frameRateTimer;
00068 delete _rtabmap;
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 }
00094 _dataMutex.unlock();
00095
00096 _userDataMutex.lock();
00097 {
00098 _userData = cv::Mat();
00099 }
00100 _userDataMutex.unlock();
00101 }
00102
00103 void RtabmapThread::setDetectorRate(float rate)
00104 {
00105 UASSERT(rate >= 0.0f);
00106 _rate = rate;
00107 }
00108
00109 void RtabmapThread::setBufferSize(int bufferSize)
00110 {
00111 UASSERT(bufferSize >= 0);
00112 _dataBufferMaxSize = bufferSize;
00113 }
00114
00115 void RtabmapThread::publishMap(bool optimized, bool full) const
00116 {
00117 std::map<int, Signature> signatures;
00118 std::map<int, Transform> poses;
00119 std::multimap<int, Link> constraints;
00120 std::map<int, int> mapIds;
00121 std::map<int, double> stamps;
00122 std::map<int, std::string> labels;
00123 std::map<int, std::vector<unsigned char> > userDatas;
00124
00125 _rtabmap->get3DMap(signatures,
00126 poses,
00127 constraints,
00128 mapIds,
00129 stamps,
00130 labels,
00131 userDatas,
00132 optimized,
00133 full);
00134
00135 this->post(new RtabmapEvent3DMap(signatures,
00136 poses,
00137 constraints,
00138 mapIds,
00139 stamps,
00140 labels,
00141 userDatas));
00142 }
00143
00144 void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
00145 {
00146 std::map<int, Signature> signatures;
00147 std::map<int, Transform> poses;
00148 std::multimap<int, Link> constraints;
00149 std::map<int, int> mapIds;
00150 std::map<int, double> stamps;
00151 std::map<int, std::string> labels;
00152 std::map<int, std::vector<unsigned char> > userDatas;
00153
00154 _rtabmap->getGraph(poses,
00155 constraints,
00156 mapIds,
00157 stamps,
00158 labels,
00159 userDatas,
00160 optimized,
00161 full);
00162
00163 this->post(new RtabmapEvent3DMap(signatures,
00164 poses,
00165 constraints,
00166 mapIds,
00167 stamps,
00168 labels,
00169 userDatas));
00170 }
00171
00172
00173 void RtabmapThread::mainLoopKill()
00174 {
00175 this->clearBufferedData();
00176
00177
00178 _dataAdded.release();
00179 }
00180
00181 void RtabmapThread::mainLoop()
00182 {
00183 State state = kStateDetecting;
00184 ParametersMap parameters;
00185
00186 _stateMutex.lock();
00187 {
00188 if(!_state.empty() && !_stateParam.empty())
00189 {
00190 state = _state.top();
00191 _state.pop();
00192 parameters = _stateParam.top();
00193 _stateParam.pop();
00194 }
00195 }
00196 _stateMutex.unlock();
00197
00198 int id = 0;
00199 std::vector<unsigned char> userData;
00200 switch(state)
00201 {
00202 case kStateDetecting:
00203 this->process();
00204 break;
00205 case kStateInit:
00206 UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
00207 Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
00208 Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
00209 UASSERT(_dataBufferMaxSize >= 0);
00210 UASSERT(_rate >= 0.0f);
00211 _rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath"));
00212 break;
00213 case kStateChangingParameters:
00214 Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
00215 Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
00216 UASSERT(_dataBufferMaxSize >= 0);
00217 UASSERT(_rate >= 0.0f);
00218 _rtabmap->parseParameters(parameters);
00219 break;
00220 case kStateReseting:
00221 _rtabmap->resetMemory();
00222 this->clearBufferedData();
00223 break;
00224 case kStateClose:
00225 if(_dataBuffer.size())
00226 {
00227 UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
00228 this->clearBufferedData();
00229 }
00230 _rtabmap->close();
00231 break;
00232 case kStateDumpingMemory:
00233 _rtabmap->dumpData();
00234 break;
00235 case kStateDumpingPrediction:
00236 _rtabmap->dumpPrediction();
00237 break;
00238 case kStateGeneratingDOTGraph:
00239 _rtabmap->generateDOTGraph(parameters.at("path"));
00240 break;
00241 case kStateGeneratingDOTLocalGraph:
00242 _rtabmap->generateDOTGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
00243 break;
00244 case kStateGeneratingTOROGraphLocal:
00245 _rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
00246 break;
00247 case kStateGeneratingTOROGraphGlobal:
00248 _rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
00249 break;
00250 case kStateCleanDataBuffer:
00251 this->clearBufferedData();
00252 break;
00253 case kStatePublishingMapLocal:
00254 this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false);
00255 break;
00256 case kStatePublishingMapGlobal:
00257 this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
00258 break;
00259 case kStatePublishingTOROGraphLocal:
00260 this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, false);
00261 break;
00262 case kStatePublishingTOROGraphGlobal:
00263 this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, true);
00264 break;
00265 case kStateTriggeringMap:
00266 _rtabmap->triggerNewMap();
00267 break;
00268 case kStateAddingUserData:
00269 _userDataMutex.lock();
00270 {
00271 userData = _userData;
00272 _userData.clear();
00273 }
00274 _userDataMutex.unlock();
00275 _rtabmap->setUserData(0, userData);
00276 break;
00277 case kStateSettingGoal:
00278 id = atoi(parameters.at("goal_id").c_str());
00279 if(id == 0 && !parameters.at("goal_label").empty() && _rtabmap->getMemory())
00280 {
00281 id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label"));
00282 }
00283 if(id <= 0 || !_rtabmap->computePath(id, true))
00284 {
00285 UERROR("Failed to set a goal to location=%d.", id);
00286 }
00287 this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath()));
00288 break;
00289 default:
00290 UFATAL("Invalid state !?!?");
00291 break;
00292 }
00293 }
00294
00295
00296 void RtabmapThread::handleEvent(UEvent* event)
00297 {
00298 if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
00299 {
00300 UDEBUG("CameraEvent");
00301 CameraEvent * e = (CameraEvent*)event;
00302 if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
00303 {
00304 this->addData(e->data());
00305 }
00306 }
00307 else if(event->getClassName().compare("OdometryEvent") == 0)
00308 {
00309 UDEBUG("OdometryEvent");
00310 OdometryEvent * e = (OdometryEvent*)event;
00311 if(e->isValid())
00312 {
00313 this->addData(e->data());
00314 }
00315 else
00316 {
00317 lastPose_.setNull();
00318 }
00319 }
00320 else if(event->getClassName().compare("UserDataEvent") == 0)
00321 {
00322 if(!_paused)
00323 {
00324 UDEBUG("UserDataEvent");
00325 bool updated = false;
00326 UserDataEvent * e = (UserDataEvent*)event;
00327 _userDataMutex.lock();
00328 if(!e->data().empty())
00329 {
00330 updated = !_userData.empty();
00331 _userData = e->data();
00332 }
00333 _userDataMutex.unlock();
00334 if(updated)
00335 {
00336 UWARN("New user data received before the last one was processed... replacing "
00337 "user data with this new one. Note that UserDataEvent should be used only "
00338 "if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
00339 }
00340 else
00341 {
00342 pushNewState(kStateAddingUserData);
00343 }
00344 }
00345 }
00346 else if(event->getClassName().compare("RtabmapEventCmd") == 0)
00347 {
00348 RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
00349 RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
00350 if(cmd == RtabmapEventCmd::kCmdInit)
00351 {
00352 ULOGGER_DEBUG("CMD_INIT");
00353 ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
00354 UASSERT(!rtabmapEvent->getStr().empty());
00355 UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
00356 pushNewState(kStateInit, parameters);
00357 }
00358 else if(cmd == RtabmapEventCmd::kCmdClose)
00359 {
00360 ULOGGER_DEBUG("CMD_CLOSE");
00361 pushNewState(kStateClose);
00362 }
00363 else if(cmd == RtabmapEventCmd::kCmdResetMemory)
00364 {
00365 ULOGGER_DEBUG("CMD_RESET_MEMORY");
00366 pushNewState(kStateReseting);
00367 }
00368 else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
00369 {
00370 ULOGGER_DEBUG("CMD_DUMP_MEMORY");
00371 pushNewState(kStateDumpingMemory);
00372 }
00373 else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
00374 {
00375 ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
00376 pushNewState(kStateDumpingPrediction);
00377 }
00378 else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
00379 {
00380 UASSERT(!rtabmapEvent->getStr().empty());
00381
00382 ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
00383 ParametersMap param;
00384 param.insert(ParametersPair("path", rtabmapEvent->getStr()));
00385 pushNewState(kStateGeneratingDOTGraph, param);
00386 }
00387 else if(cmd == RtabmapEventCmd::kCmdGenerateDOTLocalGraph)
00388 {
00389 std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
00390 UASSERT(values.size() == 3);
00391
00392 ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH");
00393 ParametersMap param;
00394 param.insert(ParametersPair("path", *values.begin()));
00395 param.insert(ParametersPair("id", *(++values.begin())));
00396 param.insert(ParametersPair("margin", *values.rbegin()));
00397 pushNewState(kStateGeneratingDOTLocalGraph, param);
00398
00399 }
00400 else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphLocal)
00401 {
00402 UASSERT(!rtabmapEvent->getStr().empty());
00403
00404 ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_LOCAL");
00405 ParametersMap param;
00406 param.insert(ParametersPair("path", rtabmapEvent->getStr()));
00407 param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
00408 pushNewState(kStateGeneratingTOROGraphLocal, param);
00409
00410 }
00411 else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphGlobal)
00412 {
00413 UASSERT(!rtabmapEvent->getStr().empty());
00414
00415 ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_GLOBAL");
00416 ParametersMap param;
00417 param.insert(ParametersPair("path", rtabmapEvent->getStr()));
00418 param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
00419 pushNewState(kStateGeneratingTOROGraphGlobal, param);
00420
00421 }
00422 else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
00423 {
00424 ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
00425 pushNewState(kStateCleanDataBuffer);
00426 }
00427 else if(cmd == RtabmapEventCmd::kCmdPublish3DMapLocal)
00428 {
00429 ULOGGER_DEBUG("CMD_PUBLISH_MAP_LOCAL");
00430 ParametersMap param;
00431 param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
00432 pushNewState(kStatePublishingMapLocal, param);
00433 }
00434 else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal)
00435 {
00436 ULOGGER_DEBUG("CMD_PUBLISH_MAP_GLOBAL");
00437 ParametersMap param;
00438 param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
00439 pushNewState(kStatePublishingMapGlobal, param);
00440 }
00441 else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphLocal)
00442 {
00443 ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_LOCAL");
00444 ParametersMap param;
00445 param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
00446 pushNewState(kStatePublishingTOROGraphLocal, param);
00447 }
00448 else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
00449 {
00450 ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_GLOBAL");
00451 ParametersMap param;
00452 param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
00453 pushNewState(kStatePublishingTOROGraphGlobal, param);
00454 }
00455 else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
00456 {
00457 ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
00458 pushNewState(kStateTriggeringMap);
00459 }
00460 else if(cmd == RtabmapEventCmd::kCmdPause)
00461 {
00462 ULOGGER_DEBUG("CMD_PAUSE");
00463 _paused = !_paused;
00464 }
00465 else if(cmd == RtabmapEventCmd::kCmdGoal)
00466 {
00467 ULOGGER_DEBUG("CMD_GOAL");
00468 ParametersMap param;
00469 param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
00470 param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
00471 pushNewState(kStateSettingGoal, param);
00472 }
00473 else
00474 {
00475 UWARN("Cmd %d unknown!", cmd);
00476 }
00477 }
00478 else if(event->getClassName().compare("ParamEvent") == 0)
00479 {
00480 ULOGGER_DEBUG("changing parameters");
00481 pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
00482 }
00483 }
00484
00485
00486
00487
00488 void RtabmapThread::process()
00489 {
00490 SensorData data;
00491 getData(data);
00492 if(data.isValid() && _state.empty())
00493 {
00494 if(_rtabmap->getMemory())
00495 {
00496 if(_rtabmap->process(data))
00497 {
00498 Statistics stats = _rtabmap->getStatistics();
00499 stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
00500 ULOGGER_DEBUG("posting statistics_ event...");
00501 this->post(new RtabmapEvent(stats));
00502 }
00503 }
00504 else
00505 {
00506 UERROR("RTAB-Map is not initialized! Ignoring received data...");
00507 }
00508 }
00509 }
00510
00511 void RtabmapThread::addData(const SensorData & sensorData)
00512 {
00513 if(!_paused)
00514 {
00515 if(!sensorData.isValid())
00516 {
00517 ULOGGER_ERROR("data not valid !?");
00518 return;
00519 }
00520
00521 if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
00522 {
00523 UWARN("Odometry is reset (identity pose detected). Increment map id!");
00524 pushNewState(kStateTriggeringMap);
00525 _rotVariance = 0;
00526 _transVariance = 0;
00527 }
00528
00529 lastPose_ = sensorData.pose();
00530 if(sensorData.poseRotVariance() > _rotVariance)
00531 {
00532 _rotVariance = sensorData.poseRotVariance();
00533 }
00534 if(sensorData.poseTransVariance() > _transVariance)
00535 {
00536 _transVariance = sensorData.poseTransVariance();
00537 }
00538
00539 if(_rate>0.0f)
00540 {
00541 if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
00542 {
00543 return;
00544 }
00545 }
00546 _frameRateTimer->start();
00547
00548 bool notify = true;
00549 _dataMutex.lock();
00550 {
00551 _dataBuffer.push_back(sensorData);
00552 if(_rotVariance <= 0)
00553 {
00554 _rotVariance = 1.0f;
00555 }
00556 if(_transVariance <= 0)
00557 {
00558 _transVariance = 1.0f;
00559 }
00560 _dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
00561 _rotVariance = 0;
00562 _transVariance = 0;
00563 while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
00564 {
00565 ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
00566 _dataBuffer.pop_front();
00567 notify = false;
00568 }
00569 }
00570 _dataMutex.unlock();
00571
00572 if(notify)
00573 {
00574 _dataAdded.release();
00575 }
00576 }
00577 }
00578
00579 void RtabmapThread::getData(SensorData & image)
00580 {
00581 ULOGGER_DEBUG("");
00582
00583 ULOGGER_INFO("waiting for data");
00584 _dataAdded.acquire();
00585 ULOGGER_INFO("wake-up");
00586
00587 _dataMutex.lock();
00588 {
00589 if(!_dataBuffer.empty())
00590 {
00591 image = _dataBuffer.front();
00592 _dataBuffer.pop_front();
00593 }
00594 }
00595 _dataMutex.unlock();
00596 }
00597
00598 void RtabmapThread::setDataBufferSize(int size)
00599 {
00600 if(size < 0)
00601 {
00602 ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
00603 _dataBufferMaxSize = 0;
00604 }
00605 else
00606 {
00607 _dataBufferMaxSize = size;
00608 }
00609 }
00610
00611 }