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

sync diagnostic: added window parameter to smooth approximated target frequency

Browse Source
This commit is contained in:
matlabbe
2023-08-29 14:01:19 -07:00
parent 28db57b29a
commit b3f33cef22
1 changed files with 48 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+27 -4
View File
@@ -6,15 +6,20 @@
#include <diagnostic_updater/diagnostic_updater.h> #include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h> #include <diagnostic_updater/publisher.h>
#include "rtabmap/utilite/ULogger.h"
namespace rtabmap_sync { namespace rtabmap_sync {
class SyncDiagnostic { class SyncDiagnostic {
public: public:
SyncDiagnostic(double tolerance = 0.1) : SyncDiagnostic(double tolerance = 0.1, int windowSize = 5) :
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)), frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
lastCallbackCalledStamp_(ros::Time::now().toSec()-1), lastCallbackCalledStamp_(ros::Time::now().toSec()-1),
targetFrequency_(0.0) targetFrequency_(0.0),
{} windowSize_(windowSize)
{
UASSERT(windowSize_ >= 1);
}
protected: protected:
void initDiagnostic( void initDiagnostic(
@@ -43,7 +48,23 @@ protected:
void tick(const ros::Time & stamp, double targetFrequency = 0) void tick(const ros::Time & stamp, double targetFrequency = 0)
{ {
frequencyStatus_.tick(); frequencyStatus_.tick();
double period = stamp.toSec() - lastCallbackCalledStamp_; double singlePeriod = stamp.toSec() - lastCallbackCalledStamp_;
window_.push_back(singlePeriod);
if(window_.size() > windowSize_)
{
window_.pop_front();
}
double period = 0.0;
if(window_.size() == windowSize_)
{
for(size_t i=0; i<window_.size(); ++i)
{
period += window_[i];
}
period /= windowSize_;
}
if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_)) if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
{ {
targetFrequency_ = 1.0/period; targetFrequency_ = 1.0/period;
@@ -73,6 +94,8 @@ private:
ros::Timer diagnosticTimer_; ros::Timer diagnosticTimer_;
double lastCallbackCalledStamp_; double lastCallbackCalledStamp_;
double targetFrequency_; double targetFrequency_;
int windowSize_;
std::deque<double> window_;
}; };