spline_motion.hpp

Go to the documentation of this file.

#pragma once
 
#include <memory>
#include <optional>
#include <vector>
#include <utility>
 
#include "motion/motion.hpp"
#include "motion/motion_pid.hpp"
#include "motion/profiled_pid_controller.hpp"
#include "motion/catmull_rom_spline.hpp"
#include "foundation/types.hpp"
 
#include <Eigen/Core>
 
namespace libstp::motion
{
    struct SplineMotionConfig
    {
        std::vector<std::pair<double, double>> waypoints_m;
        std::vector<double> headings_rad;   // empty = tangent-following; one per waypoint = explicit
        // Fraction of max speed AND travel direction via its sign:
        //  - speed_scale > 0 → drive the path forwards (nose leads).
        //  - speed_scale < 0 → drive the path in reverse. On a holonomic
        //    (mecanum/omni) drivetrain the robot HOLDS its start orientation and
        //    strafes the path backwards (nose stays put — genuine reverse). On a
        //    differential drivetrain, which cannot hold heading through a curve,
        //    it falls back to rear-first (nose points opposite the travel).
        //    Magnitude is clamped to [0.01, 1.0].
        double speed_scale{1.0};

        std::optional<double> final_heading_rad{};
    };

    struct SplineMotionTelemetry
    {
        double time_s{0.0};
        double dt{0.0};
        // Arc-length progress
        double arc_length_m{0.0};           // measured progress along spline
        double arc_target_m{0.0};           // total spline length
        // Path tracking
        double cross_track_m{0.0};          // signed perpendicular distance to path
        // Heading
        double heading_rad{0.0};            // current heading
        double target_heading_rad{0.0};     // desired heading (tangent or interpolated)
        double heading_error_rad{0.0};
        // Velocity
        double filtered_velocity_mps{0.0};
        // Commands
        double cmd_vx_mps{0.0};
        double cmd_vy_mps{0.0};
        double cmd_wz_radps{0.0};
        // Profile state
        double setpoint_position_m{0.0};
        double setpoint_velocity_mps{0.0};
        bool saturated{false};
        // Path tracking health
        bool off_path{false};               // cross-track error exceeded kOffPathThreshold
    };

    class SplineMotion final : public Motion
    {
    public:
        SplineMotion(MotionContext ctx, SplineMotionConfig config);
 
        void start() override;
        void update(double dt) override;
        [[nodiscard]] bool isFinished() const override;
 
        [[nodiscard]] const std::vector<SplineMotionTelemetry>& getTelemetry() const { return telemetry_; }
 
    private:
        void complete();

        [[nodiscard]] double targetHeadingAt(double s) const;
 
        SplineMotionConfig cfg_{};
        std::unique_ptr<CatmullRomSpline> spline_;
        double max_velocity_{0.0};
        ProfiledPIDController profiled_pid_;
        std::unique_ptr<foundation::PidController> heading_pid_;
        bool finished_{false};
 
        // Initial pose snapshot — replaces odometry().reset() at start.
        Eigen::Vector2d initial_position_m_{Eigen::Vector2d::Zero()};
        double initial_absolute_heading_rad_{0.0};  // getAbsoluteHeading() captured at start
 
        // Heading interpolation LUT: arc-length → heading (built if explicit headings)
        std::vector<double> heading_arc_lengths_;  // arc-length at each point (origin + waypoints)
        std::vector<double> all_headings_;         // heading at each point (origin=0 + user headings)
        bool use_explicit_headings_{false};
 
        // Reverse traversal (speed_scale < 0), resolved in start() once the
        // drivetrain's lateral capability is known.
        bool reverse_{false};
        bool hold_heading_{false};   // holonomic reverse: keep the start orientation
        bool rear_first_{false};     // differential reverse: nose opposite travel
 
        // Projection tracking
        double s_measured_{0.0};  // last projected arc-length (hint for next cycle)
 
        // Velocity tracking for settling detection
        double prev_s_measured_{0.0};
        double filtered_velocity_{0.0};
        static constexpr double kVelocityFilterAlpha{0.3};
        static constexpr double kSettlingVelocity{0.02}; // m/s
 
        // Profile-at-goal counter: complete after N cycles at goal with near-zero setpoint velocity
        int at_goal_cycles_{0};
        static constexpr int kAtGoalSettleCycles{30};  // 30 cycles @ 100Hz = 300ms
 
        // Saturation feedback
        double speed_scale_{1.0};
        double heading_scale_{1.0};
        int unsaturated_cycles_{0};
 
        // Off-path detection: warn once per motion instance when cross-track
        // error exceeds this threshold to avoid log spam.
        bool off_path_warned_{false};
        static constexpr double kOffPathThreshold{0.15};  // 15 cm
 
        // Telemetry
        double elapsed_time_{0.0};
        std::vector<SplineMotionTelemetry> telemetry_;
    };
}