linear_motion.hpp

Go to the documentation of this file.

#pragma once
 
#include <memory>
#include <optional>
#include <utility>
#include <vector>
 
#include <Eigen/Core>
 
#include "motion/motion.hpp"
#include "motion/motion_pid.hpp"
#include "motion/profiled_pid_controller.hpp"
#include "foundation/types.hpp"
 
namespace libstp::motion
{
    enum class LinearAxis { Forward, Lateral };

    struct LinearMotionConfig
    {
        LinearAxis axis{LinearAxis::Forward};
        double distance_m{0.0};
        double speed_scale{1.0};             // 0-1 fraction of AxisConstraints.max_velocity

        double target_heading_rad{0.0};

        bool has_distance_target{true};
    };

    struct LinearMotionTelemetry
    {
        double time_s{0.0};             // elapsed since start
        double dt{0.0};                 // this cycle's dt
        // Position
        double target_m{0.0};           // target distance
        double position_m{0.0};         // actual primary axis position
        double predicted_m{0.0};        // profile setpoint position
        double cross_track_m{0.0};      // cross-track drift
        // Errors
        double distance_error_m{0.0};   // target - actual
        double actual_error_m{0.0};     // target - actual
        double yaw_error_rad{0.0};      // heading error
        // Velocity
        double filtered_velocity_mps{0.0};
        // Commands (after clamp + scaling)
        double cmd_vx_mps{0.0};         // final commanded vx
        double cmd_vy_mps{0.0};         // final commanded vy
        double cmd_wz_radps{0.0};       // final commanded omega
        // Raw PID outputs (before clamp/scaling)
        double pid_primary_raw{0.0};    // profiled PID raw output
        double pid_cross_raw{0.0};      // cross-track PID raw output
        double pid_heading_raw{0.0};    // heading PID raw output
        // Profile state
        double setpoint_position_m{0.0};
        double setpoint_velocity_mps{0.0};
        // State
        double heading_rad{0.0};        // current heading
        double speed_scale{1.0};
        double heading_scale{1.0};
        bool saturated{false};
    };

    class LinearMotion final : public Motion
    {
    public:
        LinearMotion(MotionContext ctx, LinearMotionConfig config);
 
        void start() override;
        void update(double dt) override;
        [[nodiscard]] bool isFinished() const override;

        void startWarm(double position_offset_m, double initial_velocity_mps);

        void setSuppressHardStopOnComplete(bool suppress) { suppress_hard_stop_ = suppress; }

        [[nodiscard]] bool hasReachedDistance() const;

        [[nodiscard]] double getFilteredVelocity() const { return filtered_velocity_; }

        [[nodiscard]] const std::vector<LinearMotionTelemetry>& getTelemetry() const { return telemetry_; }

        void setOmegaOverride(double omega) { omega_override_ = omega; }
        void clearOmegaOverride() { omega_override_.reset(); }
 
    private:
        void complete();

        void captureInitialPose();

        [[nodiscard]] std::pair<double, double> projectBodyFrame() const;
 
        LinearMotionConfig cfg_{};
        double max_velocity_{0.0};  // computed from speed_scale * AxisConstraints
        std::unique_ptr<foundation::PidController> heading_pid_;
        ProfiledPIDController profiled_pid_;
        Eigen::Vector2d initial_position_m_{Eigen::Vector2d::Zero()};
        double initial_heading_rad_{0.0};
        bool finished_{false};
        bool suppress_hard_stop_{false};
        double position_offset_m_{0.0};  // subtracted from odometry in warm-start mode
        double speed_scale_{1.0};
        double heading_scale_{1.0};
        int unsaturated_cycles_{0};
 
        // Velocity tracking for settling detection
        double prev_primary_position_{0.0};
        double filtered_velocity_{0.0};
        static constexpr double kVelocityFilterAlpha{0.3};
        static constexpr double kSettlingVelocity{0.02}; // m/s - must be nearly stopped to declare done
 
        // External omega override (replaces heading PID when set)
        std::optional<double> omega_override_{};
 
        // Slew-limit state for the internal heading-hold omega output
        double prev_omega_cmd_{0.0};
 
        // Telemetry
        double elapsed_time_{0.0};
        // At 100 Hz update rate, 30 s of motion = 3000 samples × ~80 bytes
        // ≈ 240 KB. Capping at this size prevents long-running motions
        // (timeouts, condition-driven drives) from leaking memory across an
        // autonomous period. Older samples are dropped from the front; the
        // most recent window is what's interesting for tuning anyway.
        static constexpr size_t kMaxTelemetrySamples{3000};
        std::vector<LinearMotionTelemetry> telemetry_;
    };
}