OrientationInitialization.hpp

Go to the documentation of this file.

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

#ifndef POSE_ESTIMATION_ORIENTATIONINITIALIZATION_TASK_HPP
#define POSE_ESTIMATION_ORIENTATIONINITIALIZATION_TASK_HPP

#include "pose_estimation/OrientationInitializationBase.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 OrientationInitialization : public OrientationInitializationBase
    {
        friend class OrientationInitializationBase;
        static const unsigned estimator_count = 3;
    protected:
        boost::shared_ptr<pose_estimation::OrientationUKF> orientation_estimators[estimator_count];
        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 void predictionStep(const base::Time& sample_time);

        bool initializeFilter(boost::shared_ptr<pose_estimation::OrientationUKF>& filter, const Eigen::Quaterniond& orientation, const Eigen::Matrix3d& orientation_cov, const OrientationUKFConfig& filter_config);

        bool setProcessNoise(boost::shared_ptr<pose_estimation::OrientationUKF>& filter, const OrientationUKFConfig& filter_config, double sensor_delta_t);

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

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

        ~OrientationInitialization();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
    };
}

#endif