RigidBodyState.hpp

Go to the documentation of this file.

#ifndef __BASE_SAMPLES_RIGID_BODY_STATE_HH
#define __BASE_SAMPLES_RIGID_BODY_STATE_HH

#include <base/Pose.hpp>
#include <base/Time.hpp>
#include <base/Float.hpp>

#include <Eigen/Core>
#include <Eigen/LU>

namespace base { namespace samples {
    struct RigidBodyState
    {

        RigidBodyState(bool doInvalidation=true);

        base::Time time;

        std::string sourceFrame;

        std::string targetFrame;

        Position   position;
        base::Matrix3d cov_position;

        Orientation orientation;
        base::Matrix3d cov_orientation;

        base::Vector3d velocity;
        base::Matrix3d cov_velocity;

        base::Vector3d angular_velocity;
        base::Matrix3d cov_angular_velocity;

        void setTransform(const Eigen::Affine3d& transform);

         Eigen::Affine3d getTransform() const;

        void setPose(const base::Pose& pose);

        base::Pose getPose() const;

        double getYaw() const;
        
        double getPitch() const;
        
        double getRoll() const;
        
        template <int _Options>
        operator Eigen::Transform<double, 3, Eigen::Affine, _Options>() const
        {
            Eigen::Transform<double, 3, Eigen::Affine, _Options> ret;
            ret.setIdentity();
            ret.rotate(this->orientation);
            ret.translation() = this->position;
            return ret;
        }

        static RigidBodyState unknown();

        static RigidBodyState invalid();
        
        void initSane();

        void invalidate();
        
        void initUnknown();

        static bool isValidValue(base::Vector3d const& vec);
        
        static bool isValidValue(base::Orientation const& ori);
        
        static bool isKnownValue(base::Matrix3d const& cov);

        static bool isValidCovariance(base::Matrix3d const& cov);

        static bool isValidValue(base::Vector3d const& vec, int dim);
        
        static bool isValidCovariance(base::Matrix3d const& cov, int dim);
        
        static bool isKnownValue(base::Matrix3d const& cov, int dim);

        static base::Vector3d invalidValue();
        
        static base::Orientation invalidOrientation();

        static base::Matrix3d setValueUnknown();

        static base::Matrix3d invalidCovariance();
        
        bool hasValidPosition() const;
        bool hasValidPosition(int idx) const;
        bool hasValidPositionCovariance() const;
        void invalidatePosition();
        void invalidatePositionCovariance();
        
        bool hasValidOrientation() const;
        bool hasValidOrientationCovariance() const;
        void invalidateOrientation();
        void invalidateOrientationCovariance();
        
        bool hasValidVelocity() const;
        bool hasValidVelocity(int idx) const;
        bool hasValidVelocityCovariance() const;
        void invalidateVelocity();
        void invalidateVelocityCovariance();
        
        bool hasValidAngularVelocity() const;
        bool hasValidAngularVelocity(int idx) const;
        bool hasValidAngularVelocityCovariance() const;
        void invalidateAngularVelocity();
        void invalidateAngularVelocityCovariance();

        void invalidateValues(bool invPos, bool invOri, bool invVel = true,
                              bool invAngVel = true);

        void invalidateCovariances(bool invPos = true, bool invOri = true,
                                   bool invVel = true, bool invAngVel = true);
    };
}}

#endif