TimestampSynchronizer.h

Go to the documentation of this file.

#ifndef TIMESTAMPSYNCHRONIZER_H
#define TIMESTAMPSYNCHRONIZER_H

#include <string>

#include <ros/ros.h>

#include "HoltWinters.h"
#include "Mediator.h"

#include <timesync/TimesyncDebug.h>

class TimestampSynchronizer
{
public:
    class Options;

    TimestampSynchronizer(const Options defaultOptions = Options());

    double sync(double currentSensorTime, double currentRosTime, unsigned int seqNumber_external);

    void setTimeOffset(double timeOffset);
    void setMedianFilter(bool useMedianFilter, int medianFilterWindow = 0);
    void setUseHoltWinters(bool useHoltWinters);
    void setAlfa_HoltWinters(double alfa_HoltWinters);
    void setBeta_HoltWinters(double beta_HoltWinters);

    // gain scheduling in the Holt-Winters filter for faster initialization
    void setAlfa_HoltWinters_early(double alfa_HoltWinters_early);
    void setBeta_HoltWinters_early(double beta_HoltWinters_early);
    void setEarlyClamp(bool earlyClamp, int earlyClampWindow = 0);

    void reset();

    class Options {
    public:
        Options() { };
        bool useMedianFilter = true;
        int medianFilterWindow = 3000;
        bool useHoltWinters = true;
        double alfa_HoltWinters = 1e-3;
        double beta_HoltWinters = 1e-4;
        double alfa_HoltWinters_early = 1e-1;
        double beta_HoltWinters_early = 1e-2;
        bool earlyClamp = true;
        int earlyClampWindow = 500;
        double timeOffset = 0.0;
        double initialB_HoltWinters = -3e-7;
    };


private:
    double previousSensorTime_, previousRosTime_, previousOut_;

    double startRosTimeBig_;
    double startSensorTime_;
    std::unique_ptr<Mediator<double> > pmediator_;
    HoltWintersSmoothFilter holtWinters_;

    bool firstFrameSet_;
    unsigned int seqCounter_;

    void init();
    void initMediator();
    ros::NodeHandle np_;
    ros::Publisher debugPublisher_;

    Options options_;
};

#endif