OrientationEstimator.hpp

Go to the documentation of this file.

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

#ifndef POSE_ESTIMATION_ORIENTATIONESTIMATOR_TASK_HPP
#define POSE_ESTIMATION_ORIENTATIONESTIMATOR_TASK_HPP

#include "pose_estimation/OrientationEstimatorBase.hpp"
#include <boost/shared_ptr.hpp>
#include <pose_estimation/orientation_estimator/OrientationUKF.hpp>
#include <pose_estimation/orientation_estimator/OrientationUKFConfig.hpp>
#include <pose_estimation/Measurement.hpp>
#include <pose_estimation/StreamAlignmentVerifier.hpp>

namespace pose_estimation {

    class OrientationEstimator : public OrientationEstimatorBase
    {
        friend class OrientationEstimatorBase;
    protected:
        boost::shared_ptr<pose_estimation::OrientationUKF> orientation_estimator;
        boost::shared_ptr<pose_estimation::StreamAlignmentVerifier> verifier;
        Eigen::Affine3d imu_in_body_translation;
        Eigen::Quaterniond imu_in_body_rotation;
        Eigen::Matrix3d cov_angular_velocity;
        Eigen::Matrix3d cov_acceleration;
        Eigen::Matrix3d cov_velocity_unknown;
        base::Time last_velocity_sample_time;
        bool velocity_unknown;
        States last_state;
        States new_state;


        virtual void imu_sensor_samplesTransformerCallback(const base::Time &ts, const ::base::samples::IMUSensors &imu_sensor_samples_sample);
        virtual void velocity_samplesTransformerCallback(const base::Time &ts, const ::base::samples::RigidBodyState &velocity_samples_sample);

        virtual bool addHeadingOffset(double heading_offset, double variance);

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

        bool initializeFilter(const Eigen::Quaterniond& orientation, const Eigen::Matrix3d& orientation_cov, const OrientationUKFConfig& filter_config);

        bool setProcessNoise(const OrientationUKFConfig& filter_config, double sensor_delta_t);

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

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

        ~OrientationEstimator();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
    };
}

#endif