i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +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
+47 -24
View File
@@ -6,53 +6,74 @@
#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(
const std::string & topic, const std::string & topic,
const std::string & topicsNotReceivedWarningMsg, const std::string & topicsNotReceivedWarningMsg,
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>()) std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
{ {
topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg; topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg;
std::list<std::string> strList = uSplit(topic, '/'); std::list<std::string> strList = uSplit(topic, '/');
for(int i=0; i<2 && strList.size()>1; ++i) for(int i=0; i<2 && strList.size()>1; ++i)
{ {
// Assuming format is /back_camera/left/image, we want "back_camera" // Assuming format is /back_camera/left/image, we want "back_camera"
strList.pop_back(); strList.pop_back();
} }
diagnosticUpdater_.add(frequencyStatus_); diagnosticUpdater_.add(frequencyStatus_);
for(size_t i=0; i<otherTasks.size(); ++i) for(size_t i=0; i<otherTasks.size(); ++i)
{ {
diagnosticUpdater_.add(*otherTasks[i]); diagnosticUpdater_.add(*otherTasks[i]);
} }
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/")); diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
diagnosticUpdater_.force_update(); diagnosticUpdater_.force_update();
diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this); diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this);
} }
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_;
if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
{ window_.push_back(singlePeriod);
targetFrequency_ = 1.0/period; 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_))
{
targetFrequency_ = 1.0/period;
}
else if(targetFrequency>0) else if(targetFrequency>0)
{ {
targetFrequency_ = targetFrequency; targetFrequency_ = targetFrequency;
} }
lastCallbackCalledStamp_ = stamp.toSec(); lastCallbackCalledStamp_ = stamp.toSec();
} }
private: private:
@@ -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_;
}; };