odometry_snapshot.hpp

Go to the documentation of this file.

#pragma once
 
#include "hal/odometry.hpp"
 
#include <cmath>
#include <numbers>
 
namespace libstp::autotune
{
    struct OdometrySnapshot
    {
        double x{0.0};
        double y{0.0};
        double heading{0.0};
 
        static OdometrySnapshot capture(const odometry::IOdometry& odom)
        {
            const auto pose = odom.getPose();
            return {static_cast<double>(pose.position.x()),
                    static_cast<double>(pose.position.y()),
                    static_cast<double>(pose.heading)};
        }

        [[nodiscard]] std::pair<double, double>
        projectBodyFrame(const odometry::IOdometry& odom) const
        {
            const auto pose = odom.getPose();
            const double dx = static_cast<double>(pose.position.x()) - x;
            const double dy = static_cast<double>(pose.position.y()) - y;
            const double cos_h = std::cos(heading);
            const double sin_h = std::sin(heading);
            const double forward = dx * cos_h + dy * sin_h;
            const double lateral = -dx * sin_h + dy * cos_h;
            return {forward, lateral};
        }

        [[nodiscard]] double headingDelta(const odometry::IOdometry& odom) const
        {
            const double current = odom.getHeading();
            return std::remainder(current - heading, 2.0 * std::numbers::pi);
        }
    };
}