odometry.hpp

Go to the documentation of this file.

#pragma once
 
#include "foundation/types.hpp"
#include <Eigen/Core>
 
namespace libstp::odometry
{
    struct DistanceFromOrigin
    {
        double forward;

        double lateral;

        double straight_line;
    };

    enum class OdometrySource
    {
        Internal,
        CalibrationBoard,
    };
 
    struct IOdometry
    {
        virtual ~IOdometry() = default;

        virtual void update(double dt) = 0;

        [[nodiscard]] virtual foundation::Pose getPose() const = 0;

        [[nodiscard]] virtual DistanceFromOrigin getDistanceFromOrigin() const = 0;

        [[nodiscard]] virtual double getHeading() const = 0;

        [[nodiscard]] virtual double getHeadingError(double target_heading_rad) const = 0;

        [[nodiscard]] virtual double getAbsoluteHeading() const = 0;

        [[nodiscard]] virtual double getPathLength() const = 0;

        virtual void reset() = 0;

        virtual void setPreferredSource(OdometrySource source)
        {
            (void)source;
        }

        [[nodiscard]] virtual OdometrySource getPreferredSource() const
        {
            return OdometrySource::Internal;
        }

        [[nodiscard]] virtual OdometrySource getActiveSource() const
        {
            return OdometrySource::Internal;
        }

        [[nodiscard]] virtual foundation::Pose getPoseFromSource(OdometrySource source) const
        {
            (void)source;
            return getInternalPose();
        }

        [[nodiscard]] virtual bool isSourceAvailable(OdometrySource source) const
        {
            return source == OdometrySource::Internal;
        }

        [[nodiscard]] virtual foundation::Pose getInternalPose() const
        {
            return getPose();
        }

        [[nodiscard]] virtual double getInternalHeading() const
        {
            return getHeading();
        }

        [[nodiscard]] virtual DistanceFromOrigin getInternalDistanceFromOrigin() const
        {
            return getDistanceFromOrigin();
        }
    };
}