PlatformGlobalTrajectoryController.cpp

Go to the documentation of this file.

#include "PlatformGlobalTrajectoryController.h"
 
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <string>
 
#include <Ice/Current.h>
#include <IceUtil/Time.h>
 
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h> // IWYU pragma: keep
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
 
namespace armarx::navigation::platform_controller::platform_global_trajectory
{
    const NJointControllerRegistration<Controller> Registration(
        common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory));
 
    namespace
    {
        /// Nominal rates of the two loops, used only to turn a diagnostics duration into a
        /// sample count. `rtRun` is driven by the RobotUnit; `additionalTask` by `CycleUtil(10)`.
        /// A slower loop than this only means the ring holds *more* than the configured
        /// duration; `meta.json` reports the span actually recorded.
        constexpr float rtRate = 1000.F; // [Hz]
        constexpr float controlRate = 100.F; // [Hz]
 
        float
        yawOf(const Eigen::Isometry3f& pose)
        {
            return std::atan2(pose.linear()(1, 0), pose.linear()(0, 0));
        }
    } // namespace
 
    Controller::Controller(const RobotUnitPtr& robotUnit,
                           const NJointControllerConfigPtr& config,
                           const VirtualRobot::RobotPtr&)
    {
        ARMARX_IMPORTANT << "Creating "
                         << common::ControllerTypeNames.to_name(
                                common::ControllerType::PlatformGlobalTrajectory);
        // config
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        // ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
 
        ARMARX_CHECK_EXPRESSION(robotUnit);
 
        const auto robot = useSynchronizedRtRobot();
 
        // Control target
        {
            const std::string controlTargetName = robotUnit->getRobotPlatformName();
            platformName_ = controlTargetName;
 
            ARMARX_INFO << "Using control target " << controlTargetName;
            auto* ct = useControlTarget(controlTargetName, ControlModes::HolonomicPlatformVelocity);
            ARMARX_CHECK_NOT_NULL(ct) << "Cannot use control target " << QUOTED(controlTargetName);
 
            platformTarget = ct->asA<ControlTargetHolonomicPlatformVelocity>();
 
            ARMARX_CHECK_EXPRESSION(platformTarget)
                << "The actuator " << controlTargetName << " has no control mode "
                << ControlModes::HolonomicPlatformVelocity;
 
            const auto* sv = useSensorValue(controlTargetName);
            ARMARX_CHECK_NOT_NULL(sv) << "No sensor value for " << QUOTED(controlTargetName);
 
            platformSensor = sv->asA<SensorValueHolonomicPlatformVelocity>();
            ARMARX_CHECK_EXPRESSION(platformSensor)
                << "The sensor value for " << controlTargetName << " has no platform velocity";
        }
 
 
        const auto configData = ::armarx::fromAron<arondto::Config, Config>(cfg->config);
        const auto trajectoryFollowingControllerParams = configData.params;
 
        reinitTripleBuffer({});
        configBuffer_updateConfigToAdditionalTask.reinitAllBuffers(configData);
        configBuffer_updateConfigToOnPublish.reinitAllBuffers(configData);
        configBuffer_updateConfigToDiagnostics.reinitAllBuffers(configData);
 
        trajectoryFollowingController.emplace(trajectoryFollowingControllerParams);
 
        rtMaxLinearAcceleration.store(trajectoryFollowingControllerParams.maxLinearAcceleration);
        rtMaxAngularAcceleration.store(trajectoryFollowingControllerParams.maxAngularAcceleration);
 
        // See `rtMaxSlewDeltaT`. On the real robot the loop is ~1 ms, so 2 ms only ever guards
        // the first cycle after an activation. In simulation it is ~10 ms, where that cap binds
        // every cycle and throttles the slew to a fifth of the configured acceleration; a bound
        // far above any real cycle keeps it a guard rather than a limit there.
        isSimulation_ = robotUnit->isSimulation();
        rtMaxSlewDeltaT = isSimulation_ ? 1.0F : 0.002F;
 
        ARMARX_INFO << "Slew dt bound: " << rtMaxSlewDeltaT << " s ("
                    << (isSimulation_ ? "simulation" : "real robot") << ").";
 
        // The rings are sized here and never again: `rtRun` writes into them, so a later resize
        // would allocate on the real-time thread. `updateConfig` therefore ignores the
        // diagnostics block -- switching this on is a config edit plus a restart.
        diagnosticsParams_ = configData.diagnostics;
 
        if (diagnosticsParams_.enabled)
        {
            const auto capacity = [](const float seconds, const float rate) -> std::size_t
            { return static_cast<std::size_t>(std::max(0.F, seconds) * rate); };
 
            rtSamples_.init(capacity(diagnosticsParams_.rtDurationSeconds, rtRate));
            controlSamples_.init(capacity(diagnosticsParams_.controlDurationSeconds, controlRate));
 
            recordedTrajectories.reserve(
                static_cast<std::size_t>(std::max(0, diagnosticsParams_.maxTrajectoryRevisions)));
 
            ARMARX_IMPORTANT << "[nav-diag] Execution diagnostics ENABLED: keeping "
                             << rtSamples_.capacity() << " real-time and "
                             << controlSamples_.capacity() << " control samples, dumping to "
                             << diagnosticsParams_.outputDirectory
                             << " when a navigation request finishes.";
        }
        else
        {
            // Reported even when off, so "was it recording?" is one grep rather than an inference
            // from silence -- which is also what an outdated binary looks like.
            ARMARX_IMPORTANT << "[nav-diag] Execution diagnostics DISABLED. Set "
                                "`diagnostics.enabled` in the controller config this controller "
                                "was built from and restart the navigator to record a run.";
        }
 
        ARMARX_INFO << "Init done.";
    }
 
    std::string
    Controller::getClassName(const Ice::Current& iceCurrent) const
    {
        return armarx::navigation::common::ControllerTypeNames.to_name(
            armarx::navigation::common::ControllerType::PlatformGlobalTrajectory);
    }
 
    void
    Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                      const IceUtil::Time& timeSinceLastIteration)
    {
        rtUpdateControlStruct();
 
        // `additionalTask()` produces the twist at 100 Hz (`CycleUtil c(10)` in
        // `onInitNJointController`) while this runs at 1 kHz, so copying it straight through
        // hands the device a staircase that the joint controller's ramp then has to absorb.
        // Walking towards it keeps the command continuous between non-RT updates.
        //
        // This is a bridge, not a limit: the joint controller's ramp remains the authoritative
        // bound, because the same control mode is shared with the platform unit and the
        // gamepad. The rates default to that ramp's own values, so in steady motion this never
        // binds -- it only takes the edges off, which is where the jerk was.
        const float dt = static_cast<float>(timeSinceLastIteration.toSecondsDouble());
 
        // Watchdog. `additionalTask()` publishes at 100 Hz; if its sequence number stops
        // advancing the task is gone (or wedged) and `rtGetControlStruct()` is a frozen value
        // that would otherwise be executed forever. Ramp to zero instead of holding it -- via the
        // same slew, so this is a controlled stop rather than a step to zero.
        constexpr float staleAfterSeconds = 0.1F; // 10x the 100 Hz task period
 
        const std::uint64_t sequence = additionalTaskSequence.load(std::memory_order_acquire);
 
        if (sequence != rtLastSequence)
        {
            rtLastSequence = sequence;
            rtSecondsSinceTargetUpdate = 0.F;
            controlTargetStaleCycles.store(0, std::memory_order_relaxed);
        }
        else
        {
            rtSecondsSinceTargetUpdate += std::max(0.F, dt);
        }
 
        const bool targetIsStale = rtSecondsSinceTargetUpdate > staleAfterSeconds;
 
        if (targetIsStale)
        {
            // No logging here: this is the 1 kHz real-time thread. `onPublish` reports it.
            controlTargetStaleCycles.fetch_add(1, std::memory_order_relaxed);
            rtSlewTowards(Target{}, dt);
        }
        else
        {
            rtSlewTowards(rtGetControlStruct(), dt);
        }
 
        // update control devices
        platformTarget->velocityX = rtCommandedTwist.linear.x();
        platformTarget->velocityY = rtCommandedTwist.linear.y();
        platformTarget->velocityRotation = rtCommandedTwist.angular;
 
        // read data (for non-rt)
        const std::int64_t timestampUs = sensorValuesTimestamp.toMicroSeconds();
 
        // Read once into a local: `commitWrite()` swaps the write and hidden buffers, so
        // `getWriteBuffer()` afterwards names a *different* buffer holding an older cycle.
        Eigen::Isometry3f global_T_robot;
        global_T_robot.matrix() = rtGetRobot()->getGlobalPose();
 
        robotStateBuffer_rtToAdditionalTask.getWriteBuffer().global_T_robot = global_T_robot;
        robotStateBuffer_rtToAdditionalTask.getWriteBuffer().timestampUs = timestampUs;
        robotStateBuffer_rtToAdditionalTask.commitWrite();
 
        // Diagnostics. Inert unless enabled: `record` returns immediately on a zero-capacity
        // ring. When enabled it is a single assignment into preallocated storage plus one atomic
        // store -- no allocation, no lock, no I/O, which is what this thread requires.
        if (rtSamples_.capacity() > 0)
        {
            // Deliberately the pre-slew value the RT thread is acting on, so the recorded target
            // matches what the watchdog decided, not what the 100 Hz task last wrote.
            const Target target = targetIsStale ? Target{} : rtGetControlStruct();
 
            diagnostics::RtSample sample;
            sample.timestampUs = timestampUs;
            sample.episode = diagnosticsEpisode.load(std::memory_order_relaxed);
            sample.sequence = sequence;
            sample.x = global_T_robot.translation().x();
            sample.y = global_T_robot.translation().y();
            sample.yaw = yawOf(global_T_robot);
            sample.vMeasX = platformSensor->velocityX;
            sample.vMeasY = platformSensor->velocityY;
            sample.wMeas = platformSensor->velocityRotation;
            sample.vTgtX = target.linear.x();
            sample.vTgtY = target.linear.y();
            sample.wTgt = target.angular;
            sample.vCmdX = rtCommandedTwist.linear.x();
            sample.vCmdY = rtCommandedTwist.linear.y();
            sample.wCmd = rtCommandedTwist.angular;
            sample.dt = dt;
            sample.stale = targetIsStale;
            sample.slewLimited = rtSlewLimited;
 
            rtSamples_.record(sample);
        }
    }
 
    void
    Controller::rtSlewTowards(const Target& target, float dt)
    {
        // Clamped for the same reason the device ramp clamps its own: the first cycle after an
        // activation reports whatever elapsed since the controller was built, and an unbounded
        // dt would let the whole step through in one go -- exactly what this exists to avoid.
        //
        // The bound has to sit above the loop period or it stops being a guard and becomes the
        // acceleration limit itself; `rtMaxSlewDeltaT` is chosen for the loop this controller is
        // actually running in.
        dt = std::max(0.F, std::min(dt, rtMaxSlewDeltaT));
 
        // Both bounds are forced non-negative. The rates come from the aron config, so a
        // negative one is reachable by configuration alone, and neither use below survives it:
        // the linear branch would scale the step by a negative factor and drive the command
        // *away* from its target, and the angular one is an interval whose ends would be
        // reversed. `std::max` also absorbs a NaN rate, which would otherwise reach both.
        const float linearBound =
            std::max(0.F, rtMaxLinearAcceleration.load(std::memory_order_relaxed) * dt);
        const float angularBound =
            std::max(0.F, rtMaxAngularAcceleration.load(std::memory_order_relaxed) * dt);
 
        // The norm of the linear change is bounded, not each axis: two independent bounds would
        // rotate the commanded direction while the slew runs, which is the same reason the
        // device ramp couples vx and vy (`directionPreservingRamp`).
        const Eigen::Vector2f linearDelta = target.linear - rtCommandedTwist.linear;
        const float linearDistance = linearDelta.norm();
 
        rtCommandedTwist.linear +=
            linearDistance > linearBound
                ? Eigen::Vector2f{linearDelta * (linearBound / linearDistance)}
                : linearDelta;
 
        // Not `std::clamp`: its interval precondition makes a reversed range undefined
        // behaviour with no diagnostic, which is the wrong failure mode in the RT thread.
        const float angularDelta = target.angular - rtCommandedTwist.angular;
 
        rtCommandedTwist.angular += std::max(-angularBound, std::min(angularDelta, angularBound));
 
        // Recorded rather than derived offline: reconstructing it from the command alone needs
        // the rates *and* the clamped dt, and gets the equality case wrong.
        rtSlewLimited = linearDistance > linearBound or std::abs(angularDelta) > angularBound;
    }
 
    void
    Controller::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                             const Ice::Current& iceCurrent)
    {
        // TODO maybe update pid controller
 
        const auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
 
        rtMaxLinearAcceleration.store(updateConfig.params.maxLinearAcceleration);
        rtMaxAngularAcceleration.store(updateConfig.params.maxAngularAcceleration);
 
        configBuffer_updateConfigToAdditionalTask.getWriteBuffer() = updateConfig;
        configBuffer_updateConfigToAdditionalTask.commitWrite();
 
        configBuffer_updateConfigToOnPublish.getWriteBuffer() = updateConfig;
        configBuffer_updateConfigToOnPublish.commitWrite();
 
        configBuffer_updateConfigToDiagnostics.getWriteBuffer() = updateConfig;
        configBuffer_updateConfigToDiagnostics.commitWrite();
 
        // Released after the config writes, so the 100 Hz task cannot see a new revision number
        // ahead of the trajectory it refers to.
        trajectoryRevision.fetch_add(1, std::memory_order_release);
 
        // The rest of `updateConfig.diagnostics` stays ignored -- the rings were sized in the
        // constructor and the RT thread writes into them -- but `dumpRequest` is a request, not a
        // size, and it is what makes the record independent of whether this controller is ever
        // deactivated. The navigator bumps it when the goal is reached.
        if (diagnosticsParams_.enabled and updateConfig.diagnostics.dumpRequest != lastDumpRequest)
        {
            lastDumpRequest = updateConfig.diagnostics.dumpRequest;
            diagnosticsDumpRequested.store(true, std::memory_order_release);
        }
 
        ARMARX_VERBOSE << "Trajectory with " << updateConfig.targets.trajectory.points().size();
    }
 
    void
    Controller::additionalTask()
    {
        ARMARX_TRACE;
        ARMARX_CHECK(trajectoryFollowingController.has_value());
 
        const auto& configBuffer =
            configBuffer_updateConfigToAdditionalTask.getUpToDateReadBuffer();
 
 
        // if trajectory is empty, set velocity to 0
        if (configBuffer.targets.trajectory.points().empty())
        {
            ARMARX_INFO << deactivateSpam(1) << "Trajectory is empty!";
 
            filteredTwist.reset();
 
            getWriterControlStruct().reset();
            writeControlStruct();
            additionalTaskSequence.fetch_add(1, std::memory_order_release);
            return;
        }
 
        // update controller
        ARMARX_TRACE;
 
        // make sure the parameters are up to date
        // i.e. velocityFactor and limits, as they can be changed on the fly
        trajectoryFollowingController->updateParams(configBuffer.params);
 
        // run the controller, resulting in the required twist and other values
        const armarx::navigation::traj_ctrl::global::TrajectoryControllerResult result =
            trajectoryFollowingController->control(
                configBuffer.targets.trajectory,
                robotStateBuffer_rtToAdditionalTask.getUpToDateReadBuffer().global_T_robot);
 
        // low-pass filter the twist
        {
            const float alpha = configBuffer.params.alpha;
            filteredTwist.linear =
                alpha * filteredTwist.linear + (1. - alpha) * result.twist.linear.head<2>();
            filteredTwist.angular =
                alpha * filteredTwist.angular + (1. - alpha) * result.twist.angular.z();
        }
 
        // store result
        getWriterControlStruct() = filteredTwist;
        ARMARX_TRACE;
        writeControlStruct();
 
        // Tells the RT watchdog this thread is still alive. Released after the write so the RT
        // thread cannot observe a fresh sequence number ahead of the twist it refers to.
        additionalTaskSequence.fetch_add(1, std::memory_order_release);
 
        // store results (onPublish)
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().target = filteredTwist;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().dropPointVelocity =
            result.dropPoint.velocity;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().currentOrientation =
            result.currentOrientation;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().desiredOrientation =
            result.desiredOrientation;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().orientationError =
            result.orientationError;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().positionError =
            result.positionError;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().isFinalSegment =
            result.isFinalSegment;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().ffAngular = result.ffAngular;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().cappedFfVel = result.cappedFfVel;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().projectionIndex =
            result.projectionIndex;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().angularFeedforwardSaturated =
            result.angularFeedforwardSaturated;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().global_T_robot =
            robotStateBuffer_rtToAdditionalTask.getReadBuffer().global_T_robot;
        targetBuffer_additionalTaskToOnPublish.commitWrite();
 
        recordControlSample(result, filteredTwist, configBuffer.targets.trajectory,
                            configBuffer.params);
    }
 
    void
    Controller::recordControlSample(const traj_ctrl::global::TrajectoryControllerResult& result,
                                    const Twist2D& commandedTwist,
                                    const core::GlobalTrajectory& trajectory,
                                    const traj_ctrl::global::TrajectoryFollowingControllerParams&
                                        params)
    {
        if (controlSamples_.capacity() == 0)
        {
            return;
        }
 
        const RobotState& robotState = robotStateBuffer_rtToAdditionalTask.getReadBuffer();
 
        // The trajectory the samples below refer to. Copied on change rather than at dump time:
        // the navigator replans during a request, so the last one is not the one most of the
        // episode was tracking.
        const std::uint64_t revision = trajectoryRevision.load(std::memory_order_acquire);
 
        // Shared with the dump thread. Held only around the bookkeeping, never around file I/O,
        // and the common case is an uncontended lock on a branch that does nothing.
        const std::scoped_lock lock{diagnosticsTrajectoryMutex};
 
        if (revision != lastRecordedTrajectoryRevision or recordedTrajectories.empty())
        {
            lastRecordedTrajectoryRevision = revision;
 
            // Every `updateConfig` bumps the revision, including one that carries nothing but a
            // dump request, so the revision alone would fill the list with duplicates. Compared
            // on size and end point, the same cheap identity the control law uses to notice a new
            // trajectory.
            const std::size_t size = trajectory.points().size();
            const Eigen::Vector3f end =
                trajectory.points().empty()
                    ? Eigen::Vector3f{Eigen::Vector3f::Zero()}
                    : Eigen::Vector3f{trajectory.points().back().waypoint.pose.translation()};
 
            const bool unchanged = not recordedTrajectories.empty() and
                                   size == lastRecordedTrajectorySize and
                                   end == lastRecordedTrajectoryEnd;
 
            if (unchanged)
            {
                // Nothing to record, but the samples still belong to the entry already there.
                lastRecordedTrajectoryRevision = recordedTrajectories.back().revision;
            }
            else if (recordedTrajectories.size() <
                     static_cast<std::size_t>(
                         std::max(0, diagnosticsParams_.maxTrajectoryRevisions)))
            {
                recordedTrajectories.push_back({.revision = revision, .trajectory = trajectory});
                lastRecordedTrajectorySize = size;
                lastRecordedTrajectoryEnd = end;
            }
            else
            {
                droppedTrajectoryRevisions++;
            }
        }
 
        const std::uint64_t sampleRevision =
            recordedTrajectories.empty() ? revision : recordedTrajectories.back().revision;
 
        const core::Pose& reference = result.dropPoint.waypoint.pose;
 
        diagnostics::ControlSample sample;
        sample.timestampUs = robotState.timestampUs;
        sample.episode = diagnosticsEpisode.load(std::memory_order_relaxed);
        sample.sequence = additionalTaskSequence.load(std::memory_order_relaxed);
        sample.trajectoryRevision = sampleRevision;
        sample.x = robotState.global_T_robot.translation().x();
        sample.y = robotState.global_T_robot.translation().y();
        sample.yaw = yawOf(robotState.global_T_robot);
        sample.refX = reference.translation().x();
        sample.refY = reference.translation().y();
        sample.refYaw = yawOf(reference);
        sample.refVelocity = result.dropPoint.velocity;
        sample.projectionIndex = static_cast<std::uint32_t>(result.projectionIndex);
        sample.finalSegment = result.isFinalSegment;
        sample.positionError = result.positionError;
        sample.orientationError = result.orientationError;
        sample.currentOrientation = result.currentOrientation;
        sample.desiredOrientation = result.desiredOrientation;
        sample.ffAngular = result.ffAngular;
        sample.cappedFfVelocity = result.cappedFfVel;
        sample.ffSaturated = result.angularFeedforwardSaturated;
        sample.guard = static_cast<std::int32_t>(result.guard);
        sample.vRawX = result.twist.linear.x();
        sample.vRawY = result.twist.linear.y();
        sample.wRaw = result.twist.angular.z();
        sample.vFiltX = commandedTwist.linear.x();
        sample.vFiltY = commandedTwist.linear.y();
        sample.wFilt = commandedTwist.angular;
        sample.limitLinear = params.limits.linear;
        sample.limitAngular = params.limits.angular;
        sample.velocityFactor = params.velocityFactor;
 
        controlSamples_.record(sample);
    }
 
    void
    Controller::writeDiagnosticsDump()
    {
        diagnostics::Episode record;
 
        // Everything shared with the control task is taken here, and the episode is closed in the
        // same critical section: after the counter is bumped the control task and the RT thread
        // record into the next episode, so the snapshot below cannot miss a sample or take one
        // that belongs to the next request.
        const std::uint64_t episode = [&]
        {
            const std::scoped_lock lock{diagnosticsTrajectoryMutex};
 
            const std::uint64_t current = diagnosticsEpisode.load(std::memory_order_acquire);
 
            record.trajectories = std::move(recordedTrajectories);
            record.trajectoryRevisionsDropped = droppedTrajectoryRevisions;
 
            recordedTrajectories.clear();
            lastRecordedTrajectoryRevision = 0;
            lastRecordedTrajectorySize = 0;
            lastRecordedTrajectoryEnd = Eigen::Vector3f::Zero();
            droppedTrajectoryRevisions = 0;
 
            // A dump ends the episode, whatever asked for it. Without this a controller that
            // stays active across two navigation requests would write the second request's
            // record with the first one's samples still in it.
            diagnosticsEpisode.fetch_add(1, std::memory_order_release);
 
            // Latched here rather than after the file is written. The navigator deactivates the
            // controller within milliseconds of asking for the dump, while writing it takes far
            // longer; latching at the end let that deactivation queue a second request, which
            // came back as a stray dump holding the handful of cycles in between. Three of those
            // appear in the first robot recordings (`ep005`, `ep027`, `ep032`).
            diagnosticsDumpedSinceActivation.store(true, std::memory_order_release);
 
            return current;
        }();
 
        record.episode = episode;
        record.controllerInstance = getInstanceName();
        record.platform = platformName_;
        record.params = configBuffer_updateConfigToDiagnostics.getUpToDateReadBuffer().params;
 
        rtSamples_.snapshot(episode, record.rtSamples);
        controlSamples_.snapshot(episode, record.controlSamples);
 
        record.simulation = isSimulation_;
        record.slewDtBound = rtMaxSlewDeltaT;
        record.rtTruncated = rtSamples_.truncated(episode);
        record.controlTruncated = controlSamples_.truncated(episode);
        record.rtOverwritten = rtSamples_.overwritten();
        record.controlOverwritten = controlSamples_.overwritten();
        record.controlTaskExceptions = controlTaskExceptions.load(std::memory_order_relaxed);
        record.controlTargetStaleCycles = controlTargetStaleCycles.load(std::memory_order_relaxed);
 
        if (record.rtSamples.empty() and record.controlSamples.empty())
        {
            // Diagnostics are on -- `rtPostDeactivateController` only asks for a dump then -- so
            // an empty episode is worth a line: it means the controller was activated and
            // deactivated without ever running.
            ARMARX_INFO << "Execution diagnostics: episode " << episode
                        << " recorded no samples, nothing written.";
        }
        else
        {
            diagnostics::writeDump(record, diagnosticsParams_);
        }
 
    }
 
    void
    Controller::onPublish(const SensorAndControl& /*sac*/,
                          const DebugDrawerInterfacePrx& /*debugDrawer*/,
                          const DebugObserverInterfacePrx& debugObservers)
    {
        StringVariantBaseMap datafields;
 
        const auto& debugStuff = targetBuffer_additionalTaskToOnPublish.getUpToDateReadBuffer();
        const auto& config = configBuffer_updateConfigToOnPublish.getUpToDateReadBuffer();
 
 
        datafields["vx"] = new Variant(debugStuff.target.linear.x());
        datafields["vy"] = new Variant(debugStuff.target.linear.y());
        datafields["v_linear"] = new Variant(debugStuff.target.linear.norm());
        datafields["vyaw"] = new Variant(debugStuff.target.angular);
        datafields["trajectory_points"] = new Variant(config.targets.trajectory.points().size());
 
        datafields["drop_point_velocity"] = new Variant(debugStuff.dropPointVelocity);
 
        datafields["orientationError"] = new Variant(debugStuff.orientationError);
        datafields["desiredOrientation"] = new Variant(debugStuff.desiredOrientation);
        datafields["currentOrientation"] = new Variant(debugStuff.currentOrientation);
        datafields["isFinalSegment"] = new Variant(debugStuff.isFinalSegment);
 
        datafields["positionError"] = new Variant(debugStuff.positionError);
 
        datafields["ffAngular"] = new Variant(debugStuff.ffAngular);
        datafields["cappedFfVel"] = new Variant(debugStuff.cappedFfVel);
 
        datafields["global_T_robot.x"] = new Variant(debugStuff.global_T_robot.translation().x());
        datafields["global_T_robot.y"] = new Variant(debugStuff.global_T_robot.translation().y());
        datafields["global_T_robot.o"] =
            new Variant(simox::math::mat4f_to_rpy(debugStuff.global_T_robot.matrix()).z());
 
        datafields["limitLinear"] = new Variant(config.params.limits.linear);
        datafields["limitAngular"] = new Variant(config.params.limits.angular);
 
        datafields["projectionIndex"] = new Variant(static_cast<int>(debugStuff.projectionIndex));
        datafields["angularFeedforwardSaturated"] =
            new Variant(debugStuff.angularFeedforwardSaturated);
 
        const std::uint32_t staleCycles = controlTargetStaleCycles.load(std::memory_order_relaxed);
        const std::uint64_t taskExceptions = controlTaskExceptions.load(std::memory_order_relaxed);
 
        datafields["controlTargetStaleCycles"] = new Variant(static_cast<int>(staleCycles));
        datafields["controlTaskExceptions"] = new Variant(static_cast<int>(taskExceptions));
 
        // A recording that has outrun its ring is only visible in `meta.json` otherwise, i.e.
        // after the fact. Reported live so the durations can be raised before the next run.
        if (diagnosticsParams_.enabled)
        {
            datafields["diagnosticsRtOverwritten"] =
                new Variant(static_cast<int>(rtSamples_.overwritten()));
            datafields["diagnosticsControlOverwritten"] =
                new Variant(static_cast<int>(controlSamples_.overwritten()));
        }
 
        // The RT thread cannot log, so the warning is emitted here instead.
        if (staleCycles > 0 and not publishedStaleWarning)
        {
            publishedStaleWarning = true;
            ARMARX_WARNING << "[nav-guard] control-target-stale: the 100 Hz control task stopped "
                              "publishing, so the platform is being ramped to a stop. "
                           << "Swallowed control-task exceptions so far: " << taskExceptions
                           << ". The robot would previously have kept executing the last "
                              "commanded twist indefinitely.";
        }
        else if (staleCycles == 0)
        {
            publishedStaleWarning = false;
        }
 
        debugObservers->setDebugChannel(
            common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory),
            datafields);
    }
 
    void
    Controller::onInitNJointController()
    {
        ARMARX_TRACE;
        ARMARX_INFO << "PlatformGlobalTrajectoryController::onInitNJointController";
 
        runTask(
            "PlatformGlobalTrajectoryControllerAdditionalTask",
            [&]
            {
                CycleUtil c(10);
                getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                ARMARX_IMPORTANT
                    << "Create a new thread alone PlatformGlobalTrajectoryController controller";
                while (getState() == eManagedIceObjectStarted)
                {
                    if (isControllerActive() and rtReady.load())
                    {
                        ARMARX_VERBOSE << "additional task";
 
                        // `RunningTaskBase::run()` wraps this whole callback in a single
                        // try/catch, so without this an exception from one cycle leaves the loop
                        // and the thread never runs again -- while `rtRun()` keeps commanding the
                        // twist it was last handed. One bad cycle must cost one cycle.
                        try
                        {
                            additionalTask();
                        }
                        catch (const std::exception& e)
                        {
                            const std::uint64_t count =
                                controlTaskExceptions.fetch_add(1, std::memory_order_relaxed) + 1;
 
                            if (count == 1)
                            {
                                ARMARX_WARNING
                                    << "[nav-guard] control-task-exception: " << e.what()
                                    << ". The control cycle was skipped; the RT watchdog will "
                                       "ramp the platform to a stop if this persists.";
                            }
                            else
                            {
                                ARMARX_WARNING << deactivateSpam(1)
                                               << "[nav-guard] control-task-exception: " << count
                                               << " so far, latest: " << e.what();
                            }
                        }
                        catch (...)
                        {
                            controlTaskExceptions.fetch_add(1, std::memory_order_relaxed);
                            ARMARX_WARNING << deactivateSpam(1)
                                           << "[nav-guard] control-task-exception: non-standard "
                                              "exception. The control cycle was skipped.";
                        }
                    }
                    c.waitForCycleDuration();
                }
            });
 
        // A thread of its own, and not the control task above, because writing a full buffer takes
        // on the order of 100 ms. The goal-reached dump happens while the controller may still be
        // active, and a control task blocked that long stops advancing `additionalTaskSequence`
        // -- which the RT watchdog reads as a dead task after 100 ms and answers by ramping the
        // platform to a stop, with a `[nav-guard] control-target-stale` warning. A diagnostics
        // feature must not be able to manufacture a fault report.
        if (diagnosticsParams_.enabled)
        {
            runTask("PlatformGlobalTrajectoryControllerDiagnosticsTask",
                    [&]
                    {
                        CycleUtil c(50);
                        getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
 
                        while (getState() == eManagedIceObjectStarted)
                        {
                            if (diagnosticsDumpRequested.exchange(false, std::memory_order_acquire))
                            {
                                // `writeDump` swallows its own failures; this covers the
                                // assembly around it, so one bad episode cannot end the thread.
                                try
                                {
                                    writeDiagnosticsDump();
                                }
                                catch (const std::exception& e)
                                {
                                    ARMARX_WARNING << "Could not write the execution diagnostics: "
                                                   << e.what();
                                }
                                catch (...)
                                {
                                    ARMARX_WARNING << "Could not write the execution "
                                                      "diagnostics: non-standard exception.";
                                }
                            }
 
                            c.waitForCycleDuration();
                        }
                    });
        }
 
        ARMARX_INFO << "PlatformGlobalTrajectoryController::onInitNJointController done.";
    }
 
    void
    Controller::rtPreActivateController()
    {
        // additionalTask() is not executed. Thus, we can access lastTwist without mutex.
        filteredTwist.reset();
 
        // Seed the slew from what the base is actually doing, not from zero: a controller switch
        // while moving would otherwise walk the command down from a velocity the robot never
        // had. After an emergency stop this reads ~0, which is exactly the ramp-up we want.
        rtCommandedTwist.linear = {platformSensor->velocityX, platformSensor->velocityY};
        rtCommandedTwist.angular = platformSensor->velocityRotation;
 
        robotStateBuffer_rtToAdditionalTask.getWriteBuffer().global_T_robot.matrix() =
            rtGetRobot()->getGlobalPose();
        robotStateBuffer_rtToAdditionalTask.commitWrite();
 
        rtSlewLimited = false;
 
        // A plain increment, which is all the diagnostics need from this thread: every sample
        // recorded from here on carries the new id, and the dump filters on it.
        diagnosticsEpisode.fetch_add(1, std::memory_order_release);
        diagnosticsDumpedSinceActivation.store(false, std::memory_order_release);
 
        rtReady.store(true);
    }
 
    void
    Controller::rtPostDeactivateController()
    {
        rtReady.store(false);
 
        // Only a flag: writing the dump is file I/O and must not happen here. The 100 Hz task
        // thread picks it up within one cycle. This hook also runs on
        // `rtDeactivateControllerBecauseOfError`, so a fault stop is recorded too.
        //
        // `enabled` is written once, in the constructor, so reading it here is a plain load.
        //
        // Skipped when a dump was already written since this activation: that is the goal-reached
        // case, where deactivation follows within milliseconds and has nothing to add but a
        // second directory holding the few cycles in between.
        if (diagnosticsParams_.enabled and
            not diagnosticsDumpedSinceActivation.load(std::memory_order_acquire))
        {
            diagnosticsDumpRequested.store(true, std::memory_order_release);
        }
    }
 
    Controller::~Controller() = default;
 
    ::armarx::aron::data::dto::DictPtr
    Controller::getConfig(const ::Ice::Current&)
    {
        ARMARX_ERROR << "NYI";
        return nullptr;
    }
} // namespace armarx::navigation::platform_controller::platform_global_trajectory