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

RtabmapThread: using stamps in odometry messages to throttle detection rate (if stamps are not set, system time is used)

Browse Source
This commit is contained in:
matlabbe
2016-01-14 11:37:39 -05:00
parent dc2746a365
commit 6cc511cb80
3 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/RtabmapThread.h
+1
View File
@@ -106,6 +106,7 @@ private:
float _rate; float _rate;
bool _createIntermediateNodes; bool _createIntermediateNodes;
UTimer * _frameRateTimer; UTimer * _frameRateTimer;
double _previousStamp;
Rtabmap * _rtabmap; Rtabmap * _rtabmap;
bool _paused; bool _paused;
corelib/src/CameraRGB.cpp
+5 -3
View File
@@ -405,12 +405,14 @@ SensorData CameraImages::captureImage()
{ {
if(this->getImageRate() > 0.0f) if(this->getImageRate() > 0.0f)
{ {
UWARN("CameraImages: Cannot read images as fast as their timestamps (delay=%f s). Disable " UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs). Disable "
"source image rate or disable synchronization of capture time with timestamps.", _captureDelay); "source image rate or disable synchronization of capture time with timestamps.",
_captureDelay, _captureTimer.getElapsedTime());
} }
else else
{ {
UWARN("CameraImages: Cannot read images as fast as their timestamps (delay=%f s).", _captureDelay); UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs).",
_captureDelay, _captureTimer.getElapsedTime());
} }
} }
corelib/src/RtabmapThread.cpp
+5 -1
View File
@@ -48,6 +48,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_rate(Parameters::defaultRtabmapDetectionRate()), _rate(Parameters::defaultRtabmapDetectionRate()),
_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()), _createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
_frameRateTimer(new UTimer()), _frameRateTimer(new UTimer()),
_previousStamp(0.0),
_rtabmap(rtabmap), _rtabmap(rtabmap),
_paused(false), _paused(false),
lastPose_(Transform::getIdentity()), lastPose_(Transform::getIdentity()),
@@ -91,6 +92,7 @@ void RtabmapThread::clearBufferedData()
lastPose_.setIdentity(); lastPose_.setIdentity();
_rotVariance = 0; _rotVariance = 0;
_transVariance = 0; _transVariance = 0;
_previousStamp = 0;
} }
_dataMutex.unlock(); _dataMutex.unlock();
@@ -508,7 +510,8 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
bool ignoreFrame = false; bool ignoreFrame = false;
if(_rate>0.0f) if(_rate>0.0f)
{ {
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate) if((_previousStamp>0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
{ {
ignoreFrame = true; ignoreFrame = true;
} }
@@ -528,6 +531,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
else if(!ignoreFrame) else if(!ignoreFrame)
{ {
_frameRateTimer->start(); _frameRateTimer->start();
_previousStamp = odomEvent.data().stamp();
} }
lastPose_ = odomEvent.pose(); lastPose_ = odomEvent.pose();