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
+48 -25
View File
@@ -6,53 +6,74 @@
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include "rtabmap/utilite/ULogger.h"
namespace rtabmap_sync {
class SyncDiagnostic {
public:
SyncDiagnostic(double tolerance = 0.1) :
SyncDiagnostic(double tolerance = 0.1, int windowSize = 5) :
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
lastCallbackCalledStamp_(ros::Time::now().toSec()-1),
targetFrequency_(0.0)
{}
targetFrequency_(0.0),
windowSize_(windowSize)
{
UASSERT(windowSize_ >= 1);
}
protected:
void initDiagnostic(
const std::string & topic,
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;
std::list<std::string> strList = uSplit(topic, '/');
for(int i=0; i<2 && strList.size()>1; ++i)
{
// Assuming format is /back_camera/left/image, we want "back_camera"
strList.pop_back();
}
diagnosticUpdater_.add(frequencyStatus_);
for(size_t i=0; i<otherTasks.size(); ++i)
{
diagnosticUpdater_.add(*otherTasks[i]);
}
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
diagnosticUpdater_.force_update();
diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this);
std::list<std::string> strList = uSplit(topic, '/');
for(int i=0; i<2 && strList.size()>1; ++i)
{
// Assuming format is /back_camera/left/image, we want "back_camera"
strList.pop_back();
}
diagnosticUpdater_.add(frequencyStatus_);
for(size_t i=0; i<otherTasks.size(); ++i)
{
diagnosticUpdater_.add(*otherTasks[i]);
}
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
diagnosticUpdater_.force_update();
diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this);
}
void tick(const ros::Time & stamp, double targetFrequency = 0)
{
frequencyStatus_.tick();
double period = stamp.toSec() - lastCallbackCalledStamp_;
if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
{
targetFrequency_ = 1.0/period;
}
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_))
{
targetFrequency_ = 1.0/period;
}
else if(targetFrequency>0)
{
targetFrequency_ = targetFrequency;
}
lastCallbackCalledStamp_ = stamp.toSec();
lastCallbackCalledStamp_ = stamp.toSec();
}
private:
@@ -63,7 +84,7 @@ private:
if(ros::Time::now().toSec()-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
{
ROS_WARN_THROTTLE(5, topicsNotReceivedWarningMsg_.c_str());
}
}
}
private:
@@ -73,6 +94,8 @@ private:
ros::Timer diagnosticTimer_;
double lastCallbackCalledStamp_;
double targetFrequency_;
int windowSize_;
std::deque<double> window_;
};