Program Listing for File SyncDiagnostic.h

↰ Return to documentation for file (include/rtabmap_sync/SyncDiagnostic.h)

#ifndef INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
#define INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_

#include "rtabmap/utilite/UStl.h"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>

#include "rtabmap_conversions/MsgConversion.h"
#include "rtabmap/utilite/ULogger.h"

using namespace std::chrono_literals;

namespace rtabmap_sync {

class SyncDiagnostic {
    public:
        SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.1, int windowSize = 5) :
        node_(node),
        diagnosticUpdater_(node),
        frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance), node->get_clock()),
        timeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
        compositeTask_("Sync status"),
        lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
        targetFrequency_(0.0),
        windowSize_(windowSize)
    {
        UASSERT(windowSize_ >= 1);
    }

    void init(
        const std::string & topic,
        const std::string & topicsNotReceivedWarningMsg,
        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();
        }
        compositeTask_.addTask(&frequencyStatus_);
        compositeTask_.addTask(&timeStampStatus_);
        diagnosticUpdater_.add(compositeTask_);
        for(size_t i=0; i<otherTasks.size(); ++i)
        {
            diagnosticUpdater_.add(*otherTasks[i]);
        }
        diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
        diagnosticUpdater_.force_update();
        diagnosticTimer_ = node_->create_wall_timer(1s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
    }

    void tick(const rclcpp::Time & stamp, double targetFrequency = 0)
    {
        frequencyStatus_.tick();
        timeStampStatus_.tick(stamp);
        double singlePeriod = rtabmap_conversions::timestampFromROS(stamp) - 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_ = rtabmap_conversions::timestampFromROS(stamp);
    }

private:
    void diagnosticTimerCallback()
    {
        if(rtabmap_conversions::timestampFromROS(node_->now())-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
        {
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", topicsNotReceivedWarningMsg_.c_str());
        }
    }

private:
    rclcpp::Node * node_;
    std::string topicsNotReceivedWarningMsg_;
    diagnostic_updater::Updater diagnosticUpdater_;
    diagnostic_updater::FrequencyStatus frequencyStatus_;
    diagnostic_updater::TimeStampStatus timeStampStatus_;
    diagnostic_updater::CompositeDiagnosticTask compositeTask_;
    rclcpp::TimerBase::SharedPtr diagnosticTimer_;
    double lastCallbackCalledStamp_;
    double targetFrequency_;
    int windowSize_;
    std::deque<double> window_;

};

}

#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */