Diagnostics.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <atomic>
#include <cstddef>
#include <cstdint>
#include <string>
#include <utility>
#include <vector>
 
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h>
 
namespace armarx::navigation::platform_controller::diagnostics
{
 
    /**
     * @brief What the diagnostics mode records and where it writes it.
     *
     * Read once, in the controller's constructor, and never again: the buffers below are sized
     * from it and must not be resized while the real-time thread is writing into them. Turning
     * diagnostics on is therefore a config edit plus a restart of the navigator, which is the
     * right cost for a mode that exists to answer one question.
     */
    struct Params
    {
        bool enabled{false};
 
        /// How much history the 1 kHz ring keeps [s]. 60 s is ~5 MB.
        float rtDurationSeconds{60.F};
 
        /// How much history the 100 Hz ring keeps [s].
        float controlDurationSeconds{60.F};
 
        /// How many distinct trajectories one episode may record. The navigator replans during a
        /// request, so a dump that kept only the last one would mislabel most of the run.
        int maxTrajectoryRevisions{32};
 
        std::string outputDirectory{"/tmp/armarx-navigation-diagnostics"};
 
        /// Bumping this asks the controller to write the current episode and begin the next one.
        /// The only member of this struct that is read after construction.
        int dumpRequest{0};
    };
 
    /**
     * @brief One cycle of the 1 kHz real-time loop.
     *
     * Trivially copyable and fixed size: this is written from `rtRun`, so it must contain no
     * pointer, no container and nothing whose assignment could allocate.
     */
    struct RtSample
    {
        std::int64_t timestampUs{0};
        std::uint64_t episode{0};
 
        /// `additionalTaskSequence` as observed this cycle. Joins this row to `control.csv`.
        std::uint64_t sequence{0};
 
        float x{0.F};
        float y{0.F};
        float yaw{0.F};
 
        /// Measured platform velocity, base frame. The half of the overshoot question that no
        /// existing sink reports.
        float vMeasX{0.F};
        float vMeasY{0.F};
        float wMeas{0.F};
 
        /// The twist the 100 Hz task published, before the slew. Zero while the watchdog holds.
        float vTgtX{0.F};
        float vTgtY{0.F};
        float wTgt{0.F};
 
        /// What was written to the control target, i.e. after `rtSlewTowards`.
        float vCmdX{0.F};
        float vCmdY{0.F};
        float wCmd{0.F};
 
        float dt{0.F};
 
        /// The watchdog fired: the 100 Hz task stopped publishing and the command is ramping down.
        bool stale{false};
 
        /// A slew bound was binding this cycle, i.e. the command could not follow its target.
        bool slewLimited{false};
    };
 
    /// One cycle of the 100 Hz control task, i.e. one `TrajectoryFollowingController::control`.
    struct ControlSample
    {
        std::int64_t timestampUs{0};
        std::uint64_t episode{0};
        std::uint64_t sequence{0};
 
        /// Which trajectory this cycle tracked. Indexes `trajectory.json`.
        std::uint64_t trajectoryRevision{0};
 
        float x{0.F};
        float y{0.F};
        float yaw{0.F};
 
        /// The trajectory point the controller projected onto -- the reference it is tracking.
        float refX{0.F};
        float refY{0.F};
        float refYaw{0.F};
        float refVelocity{0.F};
 
        std::uint32_t projectionIndex{0};
        bool finalSegment{false};
 
        /// Distance to the *last* trajectory point, which is what the controller calls
        /// `positionError`. The cross-track error is `hypot(x - refX, y - refY)`.
        float positionError{0.F};
        float orientationError{0.F};
        float currentOrientation{0.F};
        float desiredOrientation{0.F};
 
        float ffAngular{0.F};
        float cappedFfVelocity{0.F};
        bool ffSaturated{false};
 
        /// `traj_ctrl::global::GuardVerdict` as an integer.
        std::int32_t guard{0};
 
        /// The controller's own output, base frame, before the `alpha` low-pass.
        float vRawX{0.F};
        float vRawY{0.F};
        float wRaw{0.F};
 
        /// After the low-pass, i.e. what was handed to the real-time thread.
        float vFiltX{0.F};
        float vFiltY{0.F};
        float wFilt{0.F};
 
        /// The limits in force for *this* cycle. Recorded per sample, not once in the metadata,
        /// because the safety guard drives them through `updateVelocityLimits` /
        /// `updateVelocityFactor` while the request runs -- so a snapshot taken at dump time
        /// shows whatever they were restored to and an episode that was slowed down or stopped
        /// by the guard is indistinguishable from one that was not.
        float limitLinear{0.F};
        float limitAngular{0.F};
        float velocityFactor{1.F};
    };
 
    /**
     * @brief Single-producer ring buffer whose storage is allocated exactly once.
     *
     * `record()` is the whole real-time surface: one assignment into pre-sized storage and one
     * atomic increment. No allocation, no lock, no branch beyond the modulo.
     *
     * The ring is *not* reset between episodes. Resetting the counter would race a drain that is
     * still running; instead every sample carries its episode and `snapshot()` filters, so a
     * re-activation during a dump shows up as missing samples rather than as silently
     * interleaved ones.
     */
    template <class Sample>
    class Ring
    {
    public:
        Ring() = default;
 
        explicit Ring(const std::size_t capacity) : storage_(capacity)
        {
        }
 
        /**
         * @brief Allocate the storage. Call exactly once, before the producer starts.
         *
         * Separate from the constructor because the capacity comes from the controller's config,
         * which is only available inside its constructor body -- and because `std::atomic` makes
         * the ring non-assignable, so re-seating it after the fact is not an option.
         */
        void
        init(const std::size_t capacity)
        {
            storage_.assign(capacity, Sample{});
            written_.store(0, std::memory_order_release);
        }
 
        void
        record(const Sample& sample) noexcept
        {
            if (storage_.empty())
            {
                return;
            }
 
            const std::uint64_t n = written_.load(std::memory_order_relaxed);
            storage_[static_cast<std::size_t>(n % storage_.size())] = sample;
 
            // Released after the store, so a reader that observes the count also observes the
            // sample it refers to.
            written_.store(n + 1, std::memory_order_release);
        }
 
        std::size_t
        capacity() const noexcept
        {
            return storage_.size();
        }
 
        std::uint64_t
        written() const noexcept
        {
            return written_.load(std::memory_order_acquire);
        }
 
        /// How many samples the ring has overwritten over its whole lifetime.
        ///
        /// Not the same as "this episode lost data": the ring is never reset, so this grows
        /// across every episode the controller ever ran and says nothing about whether the one
        /// being dumped fitted. Use `truncated()` for that.
        std::uint64_t
        overwritten() const noexcept
        {
            const std::uint64_t n = written();
            return n > capacity() ? n - capacity() : 0;
        }
 
        /**
         * @brief Whether samples of `episode` were overwritten before they could be read.
         *
         * True only when the ring has wrapped *and* the oldest sample still held belongs to this
         * episode -- if an older episode is still in there, nothing of this one was lost. The
         * lifetime `overwritten()` count answers a different question and reports a long-running
         * controller as lossy even when every episode fitted comfortably.
         */
        bool
        truncated(const std::uint64_t episode) const noexcept
        {
            const std::uint64_t n = written();
 
            if (storage_.empty() or n <= storage_.size())
            {
                return false;
            }
 
            const std::uint64_t oldest = n - storage_.size();
            return storage_[static_cast<std::size_t>(oldest % storage_.size())].episode == episode;
        }
 
        /**
         * @brief Copy the retained samples of `episode` into `out`, oldest first. Non-real-time.
         *
         * Safe to call while the producer is running, as long as it cannot lap the reader: the
         * entries between the write cursor and one capacity behind it are only rewritten once the
         * producer has come all the way round. At the shipped sizes that is 60 s of recording
         * against a copy measured in milliseconds. The episode filter is what keeps the result
         * meaningful across that boundary -- samples the producer adds during the copy belong to
         * the next episode and are excluded rather than mixed in.
         *
         * `out` is cleared first.
         */
        void
        snapshot(const std::uint64_t episode, std::vector<Sample>& out) const
        {
            out.clear();
 
            if (storage_.empty())
            {
                return;
            }
 
            const std::uint64_t n = written();
            const std::uint64_t first = n > storage_.size() ? n - storage_.size() : 0;
 
            out.reserve(static_cast<std::size_t>(n - first));
 
            for (std::uint64_t i = first; i < n; i++)
            {
                const Sample& sample = storage_[static_cast<std::size_t>(i % storage_.size())];
 
                if (sample.episode == episode)
                {
                    out.push_back(sample);
                }
            }
        }
 
    private:
        /// Sized in the constructor, never resized.
        std::vector<Sample> storage_;
 
        std::atomic<std::uint64_t> written_{0};
    };
 
    /// One trajectory the episode executed, with the revision the samples refer to.
    struct TrajectoryRevision
    {
        std::uint64_t revision{0};
        core::GlobalTrajectory trajectory;
    };
 
    /// Everything one dump is written from. Assembled off the real-time thread.
    struct Episode
    {
        std::uint64_t episode{0};
 
        std::string controllerInstance;
        std::string platform;
 
        traj_ctrl::global::TrajectoryFollowingControllerParams params;
 
        std::vector<RtSample> rtSamples;
        std::vector<ControlSample> controlSamples;
        std::vector<TrajectoryRevision> trajectories;
 
        /// Whether this ran against a simulated RobotUnit. Several things a reader would
        /// otherwise have to guess at follow from it: the real-time loop is an order of
        /// magnitude slower, and the platform's velocity sensor is not populated faithfully.
        bool simulation{false};
 
        /// The `dt` bound the real-time slew was actually using [s]. Recorded rather than
        /// assumed by the reader: it follows from `simulation`, and a reader that guessed would
        /// misreport every dump taken on the other one.
        float slewDtBound{0.F};
 
        /// Whether the ring overwrote samples of *this* episode before the dump read them.
        bool rtTruncated{false};
        bool controlTruncated{false};
 
        /// Lifetime overwrite counts, for context only. See `Ring::overwritten`.
        std::uint64_t rtOverwritten{0};
        std::uint64_t controlOverwritten{0};
        std::uint64_t trajectoryRevisionsDropped{0};
        std::uint64_t controlTaskExceptions{0};
        std::uint32_t controlTargetStaleCycles{0};
    };
 
    /**
     * @brief Write `episode` as `<outputDirectory>/<DateTime>-ep<NNN>/`.
     *
     * Contains `meta.json`, `trajectory.json`, `rt.csv` and `control.csv`. `trajectory.json`
     * reuses the point schema of `server/ParametrizationDump.cpp`, so the same plotting code
     * reads both.
     *
     * Never throws: a debugging aid must not be able to disturb the controller that produced it.
     * Failures are logged and swallowed.
     *
     * @return The directory written, or an empty string on failure.
     */
    std::string writeDump(const Episode& episode, const Params& params);
 
} // namespace armarx::navigation::platform_controller::diagnostics