BodyState.hpp

Go to the documentation of this file.

#ifndef __BASE_SAMPLES_BODY_STATE_HH
#define __BASE_SAMPLES_BODY_STATE_HH

#include <base/Time.hpp>
#include <base/Float.hpp>
#include <base/TransformWithCovariance.hpp>
#include <base/TwistWithCovariance.hpp>
#include <base/samples/RigidBodyState.hpp> 
namespace base { namespace samples {

    struct BodyState
    {

        BodyState(bool doInvalidation=true);

        BodyState(const base::TransformWithCovariance& pose, const base::TwistWithCovariance& velocity):
            pose(pose), velocity(velocity) {};

        base::Time time;

        base::TransformWithCovariance pose;

        base::TwistWithCovariance velocity;

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

        const base::Affine3d getPose() const;

        double getYaw() const;
        
        double getPitch() const;
        
        double getRoll() const;

        const base::Position& position() const;

        base::Position& position();

        const base::Quaterniond& orientation() const;

        base::Quaterniond& orientation();

        const base::Vector3d& linear_velocity() const;
        
        base::Position& linear_velocity();

        const base::Vector3d& angular_velocity() const;

        base::Position& angular_velocity();


        const base::Matrix6d& cov_pose() const;

        base::Matrix6d& cov_pose();

        const base::Matrix3d cov_orientation() const;

        void cov_orientation(const base::Matrix3d& cov);

        const base::Matrix3d cov_position() const;

        void cov_position(const base::Matrix3d& cov);

        const base::Matrix6d& cov_velocity() const;

        base::Matrix6d& cov_velocity();

        const base::Matrix3d cov_linear_velocity() const;

        void cov_linear_velocity(const base::Matrix3d& cov);

        const base::Matrix3d cov_angular_velocity() const;

        void cov_angular_velocity(const base::Matrix3d& cov);

        static BodyState Unknown();

        static BodyState Invalid();
        
        void initSane();

        void invalidate();
        
        void initUnknown();

        bool hasValidPose() const;
        bool hasValidPoseCovariance() const;
        void invalidatePose();
        void invalidatePoseCovariance();

        bool hasValidVelocity() const;
        bool hasValidVelocityCovariance() const;
        void invalidateVelocity();
        void invalidateVelocityCovariance();

        void invalidateValues ( bool pose = true, bool velocity = true);

        void invalidateCovariances ( bool pose = true, bool velocity = true);

        BodyState& operator=( const base::samples::RigidBodyState& rbs );


        friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans);

        BodyState composition( const BodyState& bs ) const;

        BodyState operator*( const BodyState& bs ) const;
    };

    std::ostream & operator<<(std::ostream &out, const base::samples::BodyState& bs);


}}//end namespace base::samples
#endif