RBSFilter.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */

#ifndef POSE_ESTIMATION_RBSFilter_TASK_HPP
#define POSE_ESTIMATION_RBSFilter_TASK_HPP

#include "pose_estimation/RBSFilterBase.hpp"

#include <boost/shared_ptr.hpp>
#include <pose_estimation/pose_with_velocity/PoseUKF.hpp>
#include <transformer/Transformer.hpp>
#include <pose_estimationTypes.hpp>
#include <pose_estimation/StreamAlignmentVerifier.hpp>
#include <base/samples/RigidBodyAcceleration.hpp>

namespace pose_estimation {

    class RBSFilter : public RBSFilterBase
    {
        friend class RBSFilterBase;
    protected:
        States last_state;
        States new_state;
        std::string source_frame;
        boost::shared_ptr<PoseUKF> pose_estimator;
        boost::shared_ptr<StreamAlignmentVerifier> verifier;
        base::Time last_sample_time;
        unsigned aligner_stream_failures;
        unsigned critical_aligner_stream_failures;

        bool setupFilter();

        void predictionStep(const base::Time& sample_time);

        void writeCurrentState();

        virtual bool resetState();

        void verifyStreamAlignerStatus(transformer::Transformer &trans, double verification_interval = 2.0,
                                        double drop_rate_warning = 0.5, double drop_rate_critical = 1.0)
                                    {verifyStreamAlignerStatus(trans.getStreamAlignerStatus(), verification_interval, drop_rate_warning, drop_rate_critical);}
        void verifyStreamAlignerStatus(const aggregator::StreamAlignerStatus &status, double verification_interval = 2.0,
                                       double drop_rate_warning = 0.5, double drop_rate_critical = 1.0);

        bool getSensorInBodyPose(const transformer::Transformation& sensor2body_transformer, const base::Time &ts, Eigen::Affine3d& sensorInBody);

    public:
        RBSFilter(std::string const& name = "pose_estimation::RBSFilter");

        RBSFilter(std::string const& name, RTT::ExecutionEngine* engine);

        ~RBSFilter();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
    };
}

#endif