InterventionObserver.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/>.
 *
 * @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
 */
 
#pragma once
 
 
#include <atomic>
#include <mutex>
#include <set>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
 
#include <RobotAPI/interface/components/InterventionObserverInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
#include <RobotAPI/libraries/armem_task_outcome/client/TaskOutcomeReader.h>
#include <RobotAPI/libraries/armem_task_outcome/client/TaskOutcomeWriter.h>
 
namespace armarx
{
    /**
     * @class InterventionObserverPropertyDefinitions
     * @brief
     */
    class InterventionObserverPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        InterventionObserverPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "PlatformUnitName", "Armar6IcePlatformUnit", "Name of the platform unit to use");
            defineOptionalProperty<std::string>(
                "GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
            defineOptionalProperty<std::string>(
                "EmergencyStopTopic", "EmergencyStop", "Name of the Emergency Stop Topic");
            defineOptionalProperty<std::string>(
                "AgentName", "ARMAR-7", "Agent name used in TaskOutcome commits");
            defineOptionalProperty<float>(
                "AssistanceSessionTimeoutSeconds",
                2.0f,
                "Seconds of inactivity before an assistance session is committed to memory");
        }
    };
 
    /**
     * @defgroup Component-InterventionObserver InterventionObserver
     * @ingroup RobotAPI-Components
     * @class InterventionObserver
     * @ingroup Component-InterventionObserver
     * @brief Observes human interventions during autonomous robot execution and
     *        records them as TaskOutcome entries in the failure memory.
     *
     * Listens to gamepad and emergency stop topics. When the operator suspends
     * the robot (back button / emergency stop), a SUSPENDED TaskOutcome is
     * committed whose skill name is resolved from the Skill memory's latest
     * SkillEvent. When the operator resumes (start button / emergency stop
     * release), the most recent SUSPENDED entry is updated with recovery info.
     *
     * Additionally tracks continuous gamepad usage as "assistance sessions"
     * (e.g. manual platform navigation) and commits a summary once the
     * operator stops providing input for a configurable timeout period.
     */
    class InterventionObserver :
        virtual public armarx::Component,
        virtual public InterventionObserverInterface,
        virtual public armarx::armem::client::ComponentPluginUser
    {
    public:
        InterventionObserver();
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "InterventionObserver";
        }
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    public:
        void reportGamepadState(const std::string& device,
                                const std::string& name,
                                const GamepadData& data,
                                const TimestampBasePtr& timestamp,
                                const Ice::Current& c) override;
 
        void reportEmergencyStopState(EmergencyStopState state, const Ice::Current&) override;
 
    private:
        /// Lazily connects to the TaskOutcome and Skill memory servers.
        /// Called on every incoming event; returns false if not yet available.
        bool ensureMemoryConnection();
 
        /// Queries the Skill memory's SkillEvent segment for the most recently
        /// started skill and returns its name. Falls back to "unknown".
        std::string queryLatestSkillName();
 
        // TaskOutcome memory: writer commits interventions, reader queries
        // previous SUSPENDED entries when checking for recovery.
        armem::task_outcome::client::TaskOutcomeWriter writer;
        armem::task_outcome::client::TaskOutcomeReader reader;
 
        // Skill memory reader: used to look up which skill was running when
        // an intervention (emergency stop / gamepad back) occurred.
        armem::client::Reader skillMemoryReader;
 
        std::atomic_bool memoryConnected{false};
        // Protects the one-time connection setup against concurrent event callbacks.
        std::mutex memoryConnectionMutex;
 
        // Rising-edge detection state for gamepad buttons.
        bool prevBackButton = false;
        bool prevStartButton = false;
 
        // State transition detection for emergency stop events.
        EmergencyStopState prevEmergencyStopState = EmergencyStopState::eEmergencyStopInactive;
 
        std::string agentName;
        static constexpr const char* providerName = "InterventionObserver";
 
        // Gamepad assistance session tracking
        bool assistanceSessionActive = false;
        armarx::core::time::DateTime assistanceSessionStart;
        armarx::core::time::DateTime assistanceLastActivityTime;
        std::set<std::string> assistanceActionsInSession;
        float assistanceSessionTimeoutSeconds = 2.0f;
        std::string lastGamepadDevice;
        std::string lastGamepadName;
    };
} // namespace armarx