InterventionObserver.cpp

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/>.
 *
 * @package    RobotAPI::ArmarXObjects::InterventionObserver
 * @author     Niklas Arlt ( niklas dot arlt at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "InterventionObserver.h"
 
#include <cmath>
#include <sstream>
 
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
#include <RobotAPI/libraries/armem_task_outcome/types.h>
 
namespace armarx
{
    void
    InterventionObserver::onInitComponent()
    {
        ARMARX_INFO << "oninit InterventionObserver";
 
        usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
        usingTopic(getProperty<std::string>("EmergencyStopTopic").getValue());
 
        agentName = getProperty<std::string>("AgentName").getValue();
        assistanceSessionTimeoutSeconds =
            getProperty<float>("AssistanceSessionTimeoutSeconds").getValue();
 
        // usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
 
        ARMARX_INFO << "oninit InterventionObserver end";
    }
 
    void
    InterventionObserver::onConnectComponent()
    {
        ARMARX_INFO << "onConnect InterventionObserver";
 
        // Try to connect immediately. If the memory is already up this succeeds.
        // If not, ensureMemoryConnection() will retry on every incoming event.
        ensureMemoryConnection();
    }
 
    bool
    InterventionObserver::ensureMemoryConnection()
    {
        if (memoryConnected.load())
        {
            return true;
        }
 
        // Double-checked locking: try_to_lock avoids blocking event callbacks
        // while another thread is already attempting connection.
        std::unique_lock lock(memoryConnectionMutex, std::try_to_lock);
        if (!lock.owns_lock())
        {
            return false;
        }
 
        if (memoryConnected.load())
        {
            return true;
        }
 
        try
        {
            // Use getWriter/getReader which resolve immediately (non-blocking).
            // They throw if the memory server hasn't registered yet.
            const auto memoryId = armem::MemoryID().withMemoryName("TaskOutcome");
            memoryNameSystem().getWriter(memoryId);
            memoryNameSystem().getReader(memoryId);
 
            // Memory is available — now connect writer/reader properly.
            // Since the server is already registered, waitForServer() returns immediately.
            writer.connect(memoryNameSystem());
            reader.connect(memoryNameSystem());
 
            // Connect reader for the Skill memory to query latest skill events.
            const auto skillMemoryId = armem::MemoryID().withMemoryName("Skill");
            skillMemoryReader = memoryNameSystem().useReader(skillMemoryId);
 
            memoryConnected.store(true);
            ARMARX_INFO << "InterventionObserver: Memory connection established.";
        }
        catch (...)
        {
            ARMARX_INFO << deactivateSpam(5)
                        << "InterventionObserver: TaskOutcome memory not available yet. "
                           "Will retry on next event.";
        }
 
        return memoryConnected.load();
    }
 
    void
    InterventionObserver::onDisconnectComponent()
    {
        memoryConnected = false;
    }
 
    void
    InterventionObserver::onExitComponent()
    {
        ARMARX_INFO << "exit InterventionObserver";
    }
 
    armarx::PropertyDefinitionsPtr
    InterventionObserver::createPropertyDefinitions()
    {
        auto defs = armarx::PropertyDefinitionsPtr(
            new InterventionObserverPropertyDefinitions(getConfigIdentifier()));
 
        return defs;
    }
 
    // Called on every gamepad event. Handles three concerns:
    // 1) Back button press  -> commit a SUSPENDED TaskOutcome (skill was interrupted).
    // 2) Start button press -> find the last SUSPENDED entry and mark it recovered.
    // 3) Any analog/button input -> track as an "assistance session" and commit
    //    a summary after the operator goes idle for the configured timeout.
    void
    InterventionObserver::reportGamepadState(const std::string& device,
                                             const std::string& name,
                                             const GamepadData& data,
                                             const TimestampBasePtr& timestamp,
                                             const Ice::Current& c)
    {
        // Always update rising-edge detection state, even when the memory is
        // not yet available, so we don't miss transitions or get stuck.
        const bool backRisingEdge = data.backButton && !prevBackButton;
        prevBackButton = data.backButton;
 
        const bool startRisingEdge = data.startButton && !prevStartButton;
        prevStartButton = data.startButton;
 
        if (!ensureMemoryConnection())
        {
            return;
        }
 
        // --- 1) Back button: suspend the currently running skill ---
        if (backRisingEdge)
        {
            ARMARX_INFO << "Back button pressed — committing SUSPENDED TaskOutcome";
 
            const std::string suspendedSkillName = queryLatestSkillName();
            ARMARX_INFO << "Skill suspended by gamepad back button: " << suspendedSkillName;
 
            armem::task_outcome::TaskOutcome outcome;
            outcome.taskName = suspendedSkillName;
            outcome.taskType = armem::task_outcome::TaskType::SPECIAL;
            outcome.outcomeType = armem::task_outcome::TaskOutcomeType::SUSPENDED;
            outcome.agent = agentName;
            outcome.startTime = armarx::DateTime::Now();
            outcome.endTime = armarx::DateTime::Now();
 
            armem::task_outcome::FailureInfo fi;
            fi.failureType.interruption.userStopped = true;
            outcome.failureInfo = fi;
 
            outcome.context.skillName = suspendedSkillName;
            outcome.context.additional = {
                {"source", "gamepad"},
                {"device", device},
                {"name", name},
                {"leftStickX", std::to_string(data.leftStickX)},
                {"leftStickY", std::to_string(data.leftStickY)},
                {"rightStickX", std::to_string(data.rightStickX)},
                {"rightStickY", std::to_string(data.rightStickY)},
                {"dPadX", std::to_string(data.dPadX)},
                {"dPadY", std::to_string(data.dPadY)},
                {"leftTrigger", std::to_string(data.leftTrigger)},
                {"rightTrigger", std::to_string(data.rightTrigger)},
                {"leftButton", std::to_string(data.leftButton)},
                {"rightButton", std::to_string(data.rightButton)},
                {"backButton", std::to_string(data.backButton)},
                {"startButton", std::to_string(data.startButton)},
                {"xButton", std::to_string(data.xButton)},
                {"yButton", std::to_string(data.yButton)},
                {"aButton", std::to_string(data.aButton)},
                {"bButton", std::to_string(data.bButton)},
                {"theMiddleButton", std::to_string(data.theMiddleButton)},
                {"leftStickButton", std::to_string(data.leftStickButton)},
                {"rightStickButton", std::to_string(data.rightStickButton)},
            };
 
            const bool success =
                writer.commitTaskOutcome(outcome, providerName, armem::Time::Now());
 
            if (success)
            {
                ARMARX_INFO << "Successfully committed SUSPENDED TaskOutcome for gamepad intervention.";
            }
            else
            {
                ARMARX_WARNING << "Failed to commit SUSPENDED TaskOutcome for gamepad intervention.";
            }
        }
 
        // --- 2) Start button: mark the last gamepad-caused suspension as recovered ---
        if (startRisingEdge)
        {
            ARMARX_INFO << "Start button pressed — checking for SUSPENDED entry to mark as recovered";
 
            armem::task_outcome::client::TaskOutcomeReader::Query q{
                .providerName = providerName,
                .taskTypeFilter = std::nullopt,
                .outcomeFilter = armem::task_outcome::TaskOutcomeType::SUSPENDED,
                .timestamp = armem::Time::Now(),
                .maxAge = armarx::Duration::Hours(24)};
 
            const auto result = reader.query(q);
 
            if (!result || result.outcomes.empty())
            {
                ARMARX_WARNING << "Start button pressed but no SUSPENDED TaskOutcome found in memory. Skipping recovery.";
            }
            else
            {
                // Find the most recent SUSPENDED outcome from the gamepad source.
                const armem::task_outcome::TaskOutcome* lastSuspended = nullptr;
                for (const auto& o : result.outcomes)
                {
                    auto it = o.context.additional.find("source");
                    if (it == o.context.additional.end() || it->second != "gamepad")
                    {
                        continue;
                    }
                    if (!lastSuspended || o.endTime > lastSuspended->endTime)
                    {
                        lastSuspended = &o;
                    }
                }
 
                if (!lastSuspended)
                {
                    ARMARX_WARNING << "Start button pressed but no gamepad-sourced SUSPENDED entry found. Skipping recovery.";
                }
                else
                {
                    // Re-commit the suspended entry with recovery info attached. This
                    // creates a new snapshot rather than modifying the original, so
                    // the full history (suspension + recovery) is preserved.
                    armem::task_outcome::TaskOutcome recovered = *lastSuspended;
                    recovered.couldRecover = true;
                    recovered.recoveryInfo = armem::task_outcome::RecoveryInfo{
                        .recoveryTime = armarx::DateTime::Now(),
                        .recoveryMeasure = "resume by gamepad triggered"};
 
                    const bool success =
                        writer.commitTaskOutcome(recovered, providerName, armem::Time::Now());
 
                    if (success)
                    {
                        ARMARX_INFO << "Successfully committed recovery for task '"
                                    << recovered.taskName << "'.";
                    }
                    else
                    {
                        ARMARX_WARNING << "Failed to commit recovery TaskOutcome.";
                    }
                }
            }
        }
 
        // --- 3) Assistance session tracking ---
        // Classify which gamepad inputs are active this frame, then accumulate
        // them into a session. A session starts on the first non-idle input and
        // ends (gets committed) after no input for `assistanceSessionTimeoutSeconds`.
        lastGamepadDevice = device;
        lastGamepadName = name;
 
        constexpr float deadzone = 0.1f;
 
        std::set<std::string> activeActions;
 
        if (std::abs(data.leftTrigger) > deadzone || std::abs(data.rightTrigger) > deadzone)
        {
            activeActions.insert("manual gripper control");
        }
        if (std::abs(data.leftStickX) > deadzone || std::abs(data.leftStickY) > deadzone)
        {
            activeActions.insert("manual platform navigation");
        }
        if (std::abs(data.rightStickX) > deadzone || std::abs(data.rightStickY) > deadzone)
        {
            activeActions.insert("rightStick");
        }
        if (std::abs(data.dPadX) > deadzone || std::abs(data.dPadY) > deadzone)
        {
            activeActions.insert("dPad");
        }
        if (data.aButton)
        {
            activeActions.insert("manual action button");
        }
        if (data.bButton)
        {
            activeActions.insert("manual action button");
        }
        if (data.xButton)
        {
            activeActions.insert("manual action button");
        }
        if (data.yButton)
        {
            activeActions.insert("manual action button");
        }
        if (data.leftButton)
        {
            activeActions.insert("manual bumper action");
        }
        if (data.rightButton)
        {
            activeActions.insert("manual bumper action");
        }
        if (data.theMiddleButton)
        {
            activeActions.insert("theMiddleButton");
        }
        if (data.leftStickButton)
        {
            activeActions.insert("leftStickButton");
        }
        if (data.rightStickButton)
        {
            activeActions.insert("rightStickButton");
        }
 
        const auto now = armarx::DateTime::Now();
        const bool hasInput = !activeActions.empty();
 
        if (hasInput)
        {
            if (!assistanceSessionActive)
            {
                // Start a new session
                assistanceSessionActive = true;
                assistanceSessionStart = now;
                assistanceActionsInSession = activeActions;
                ARMARX_INFO << "Assistance session started";
            }
            else
            {
                // Merge actions into existing session
                assistanceActionsInSession.insert(activeActions.begin(), activeActions.end());
            }
            assistanceLastActivityTime = now;
        }
        else if (assistanceSessionActive)
        {
            const auto elapsed = now - assistanceLastActivityTime;
            if (elapsed > armarx::Duration::SecondsDouble(assistanceSessionTimeoutSeconds))
            {
                // Commit the session
                std::ostringstream actionStr;
                bool first = true;
                for (const auto& a : assistanceActionsInSession)
                {
                    if (!first)
                    {
                        actionStr << ", ";
                    }
                    actionStr << a;
                    first = false;
                }
 
                const auto durationMs =
                    (assistanceLastActivityTime - assistanceSessionStart).toMilliSeconds();
 
                armem::task_outcome::TaskOutcome outcome;
                outcome.taskName = "GamepadIntervention";
                outcome.taskType = armem::task_outcome::TaskType::SPECIAL;
                outcome.outcomeType = armem::task_outcome::TaskOutcomeType::UNKNOWN;
                outcome.agent = agentName;
                outcome.startTime = assistanceSessionStart;
                outcome.endTime = assistanceLastActivityTime;
                outcome.context.skillName = "unknown";
                outcome.context.additional = {
                    {"source", "gamepad_assistance"},
                    {"actions", actionStr.str()},
                    {"duration_ms", std::to_string(durationMs)},
                    {"device", lastGamepadDevice},
                    {"name", lastGamepadName},
                };
 
                const bool success =
                    writer.commitTaskOutcome(outcome, providerName, armem::Time::Now());
 
                if (success)
                {
                    ARMARX_INFO << "Committed assistance session: actions=[" << actionStr.str()
                                << "], duration=" << durationMs << "ms";
                }
                else
                {
                    ARMARX_WARNING << "Failed to commit assistance session TaskOutcome.";
                }
 
                // Reset session state
                assistanceSessionActive = false;
                assistanceActionsInSession.clear();
            }
        }
    }
 
    std::string
    InterventionObserver::queryLatestSkillName()
    {
        if (!skillMemoryReader)
        {
            return "unknown";
        }
 
        try
        {
            // Query the latest snapshot of every entity in the SkillEvent segment.
            // Each entity represents one skill execution; its latest snapshot holds
            // the most recent status update for that execution.
            armem::client::query::Builder qb;
            qb.coreSegments()
                .withName("SkillEvent")
                .providerSegments()
                .all()
                .entities()
                .all()
                .snapshots()
                .latest();
 
            const auto result = skillMemoryReader.query(qb);
            if (!result.success)
            {
                ARMARX_WARNING << "Failed to query Skill memory for latest skill event.";
                return "unknown";
            }
 
            const auto* cs = result.memory.findCoreSegment("SkillEvent");
            if (!cs)
            {
                return "unknown";
            }
 
            // Among all skill executions, pick the one that was started most
            // recently — that is the skill the operator most likely intended
            // to interrupt.
            std::string latestSkillName = "unknown";
            armarx::core::time::DateTime latestTime = armarx::core::time::DateTime::Invalid();
 
            cs->forEachEntity(
                [&](const armem::wm::Entity& entity)
                {
                    if (const auto* instance = entity.findLatestInstance(); instance)
                    {
                        auto aronUpdate =
                            skills::arondto::SkillStatusUpdate::FromAron(instance->data());
                        if (!latestTime.isValid() ||
                            aronUpdate.executionStartedTimestamp > latestTime)
                        {
                            latestTime = aronUpdate.executionStartedTimestamp;
                            latestSkillName = aronUpdate.skillId.skillName;
                        }
                    }
                });
 
            return latestSkillName;
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "Exception querying Skill memory: " << e.what();
            return "unknown";
        }
    }
 
    // Called on every emergency stop state change. Only reacts to transitions:
    // INACTIVE -> ACTIVE:  commit a SUSPENDED TaskOutcome for the currently running skill.
    // ACTIVE -> INACTIVE:  find the last emergency-stop-caused suspension and mark it recovered.
    void
    InterventionObserver::reportEmergencyStopState(EmergencyStopState state, const Ice::Current&)
    {
        if (!ensureMemoryConnection())
        {
            // Still track the state so we detect the correct transition once
            // the memory becomes available.
            prevEmergencyStopState = state;
            return;
        }
 
        // Detect state transitions; ignore repeated reports of the same state.
        const bool becameActive =
            (state == EmergencyStopState::eEmergencyStopActive &&
             prevEmergencyStopState == EmergencyStopState::eEmergencyStopInactive);
        const bool becameInactive =
            (state == EmergencyStopState::eEmergencyStopInactive &&
             prevEmergencyStopState == EmergencyStopState::eEmergencyStopActive);
        prevEmergencyStopState = state;
 
        if (becameActive)
        {
            ARMARX_INFO << "Emergency stop activated — committing SUSPENDED TaskOutcome";
 
            const std::string suspendedSkillName = queryLatestSkillName();
            ARMARX_INFO << "Skill suspended by emergency stop: " << suspendedSkillName;
 
            armem::task_outcome::TaskOutcome outcome;
            outcome.taskName = suspendedSkillName;
            outcome.taskType = armem::task_outcome::TaskType::SPECIAL;
            outcome.outcomeType = armem::task_outcome::TaskOutcomeType::SUSPENDED;
            outcome.agent = agentName;
            outcome.startTime = armarx::DateTime::Now();
            outcome.endTime = armarx::DateTime::Now();
 
            armem::task_outcome::FailureInfo fi;
            fi.failureType.interruption.userStopped = true;
            outcome.failureInfo = fi;
 
            outcome.context.skillName = suspendedSkillName;
            outcome.context.additional = {
                {"source", "emergency_stop"},
                {"emergencyStopState", "active"}};
 
            const bool success =
                writer.commitTaskOutcome(outcome, providerName, armem::Time::Now());
 
            if (success)
            {
                ARMARX_INFO << "Successfully committed SUSPENDED TaskOutcome for emergency stop.";
            }
            else
            {
                ARMARX_WARNING << "Failed to commit SUSPENDED TaskOutcome for emergency stop.";
            }
        }
 
        if (becameInactive)
        {
            ARMARX_INFO << "Emergency stop released — checking for SUSPENDED entry to mark as recovered";
 
            armem::task_outcome::client::TaskOutcomeReader::Query q{
                .providerName = providerName,
                .taskTypeFilter = std::nullopt,
                .outcomeFilter = armem::task_outcome::TaskOutcomeType::SUSPENDED,
                .timestamp = armem::Time::Now(),
                .maxAge = armarx::Duration::Hours(24)};
 
            const auto result = reader.query(q);
 
            if (!result || result.outcomes.empty())
            {
                ARMARX_WARNING << "Emergency stop released but no SUSPENDED TaskOutcome found in memory. Skipping recovery.";
                return;
            }
 
            // Find the most recent SUSPENDED outcome from the emergency_stop source.
            const armem::task_outcome::TaskOutcome* lastSuspended = nullptr;
            for (const auto& o : result.outcomes)
            {
                auto it = o.context.additional.find("source");
                if (it == o.context.additional.end() || it->second != "emergency_stop")
                {
                    continue;
                }
                if (!lastSuspended || o.endTime > lastSuspended->endTime)
                {
                    lastSuspended = &o;
                }
            }
 
            if (!lastSuspended)
            {
                ARMARX_WARNING << "Emergency stop released but no emergency-stop-sourced SUSPENDED entry found. Skipping recovery.";
                return;
            }
 
            armem::task_outcome::TaskOutcome recovered = *lastSuspended;
            recovered.couldRecover = true;
            recovered.recoveryInfo = armem::task_outcome::RecoveryInfo{
                .recoveryTime = armarx::DateTime::Now(),
                .recoveryMeasure = "resume by emergency stop release"};
 
            const bool success =
                writer.commitTaskOutcome(recovered, providerName, armem::Time::Now());
 
            if (success)
            {
                ARMARX_INFO << "Successfully committed recovery for emergency stop intervention.";
            }
            else
            {
                ARMARX_WARNING << "Failed to commit recovery TaskOutcome for emergency stop.";
            }
        }
    }
 
    InterventionObserver::InterventionObserver()
    {
    }
} // namespace armarx