VelocityEstimator.hpp

Go to the documentation of this file.

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

#ifndef UWV_DYNAMIC_MODEL_VELOCITYESTIMATOR_TASK_HPP
#define UWV_DYNAMIC_MODEL_VELOCITYESTIMATOR_TASK_HPP

#include "uwv_dynamic_model/VelocityEstimatorBase.hpp"
#include <queue>
#include <deque>
#include <vector>
#include "numeric/SavitzkyGolayFilter.hpp"

namespace uwv_dynamic_model{

    class VelocityEstimator : public VelocityEstimatorBase
    {
        friend class VelocityEstimatorBase;
    protected:


        // Secondary model_simulation, use for replaying old effort data and time allying dvl data
        ModelSimulation* model_simulation2;

        // Deriving depth data for estimating vertical velocity
        // Savitzky-Golay coefficients.
        std::vector<double> filter_coeff;
        // Depth data samples
        std::deque<base::samples::RigidBodyState> queueOfDepthData;

        std::queue<std::pair<base::LinearAngular6DCommand, base::samples::RigidBodyState> > queueOfStates;
        bool getTransformation(const transformer::Transformation &transformer, Eigen::Affine3d &transformationMatrix);

        void handleStates(const base::samples::RigidBodyState &state, const base::LinearAngular6DCommand &control_input);

        void setRuntimeState();

        base::samples::RigidBodyState updatePoseWithXYVelocity(const base::samples::RigidBodyState &pose, const base::samples::RigidBodyState &dvl_sample);

         double verticalVelocityEstimation(const base::samples::RigidBodyState &depth_sample);

         base::samples::RigidBodyState replayModel(const base::samples::RigidBodyState &dvl_sample);

    public:
        VelocityEstimator(std::string const& name = "uwv_dynamic_model::VelocityEstimator", ModelSimulator simulator = DYNAMIC);

        VelocityEstimator(std::string const& name, RTT::ExecutionEngine* engine, ModelSimulator simulator = DYNAMIC);

        ~VelocityEstimator();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
    };
}

#endif