i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-09 11:17:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

Version 0.8.8: added "user_data" field in database. Added WifiMapping example. Fixed FATAL error when initializing rtabmap with no database.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-25 13:31:12 -04:00
parent 2be511813e
commit 186dc60a49
32 changed files with 1064 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/RtabmapThread.cpp
+43
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/ParamEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/UserDataEvent.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
@@ -90,6 +91,12 @@ void RtabmapThread::clearBufferedData()
_transVariance = 0;
}
_dataMutex.unlock();
_userDataMutex.lock();
{
_userData = cv::Mat();
}
_userDataMutex.unlock();
}
void RtabmapThread::setDetectorRate(float rate)
@@ -175,6 +182,7 @@ void RtabmapThread::mainLoop()
}
_stateMutex.unlock();
std::vector<unsigned char> userData;
switch(state)
{
case kStateDetecting:
@@ -243,6 +251,15 @@ void RtabmapThread::mainLoop()
case kStateTriggeringMap:
_rtabmap->triggerNewMap();
break;
case kStateAddingUserData:
_userDataMutex.lock();
{
userData = _userData;
_userData.clear();
}
_userDataMutex.unlock();
_rtabmap->setUserData(0, userData);
break;
default:
UFATAL("Invalid state !?!?");
break;
@@ -274,6 +291,32 @@ void RtabmapThread::handleEvent(UEvent* event)
lastPose_.setNull();
}
}
else if(event->getClassName().compare("UserDataEvent") == 0)
{
if(!_paused)
{
UDEBUG("UserDataEvent");
bool updated = false;
UserDataEvent * e = (UserDataEvent*)event;
_userDataMutex.lock();
if(!e->data().empty())
{
updated = !_userData.empty();
_userData = e->data();
}
_userDataMutex.unlock();
if(updated)
{
UWARN("New user data received before the last one was processed... replacing "
"user data with this new one. Note that UserDataEvent should be used only "
"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
}
else
{
pushNewState(kStateAddingUserData);
}
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;