InterventionDebugger.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::InterventionDebugger
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "InterventionDebugger.h"
 
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
namespace armarx
{
    InterventionDebugger::InterventionDebugger()
    {
    }
 
    armarx::PropertyDefinitionsPtr
    InterventionDebugger::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new InterventionDebuggerPropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    InterventionDebugger::onInitComponent()
    {
        deviceName = getProperty<std::string>("DeviceName").getValue();
        gamepadName = getProperty<std::string>("GamepadName").getValue();
 
        offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
        offeringTopic(getProperty<std::string>("EmergencyStopTopicName").getValue());
    }
 
    void
    InterventionDebugger::onConnectComponent()
    {
        gamepadTopicPrx =
            getTopic<GamepadUnitListenerPrx>(getProperty<std::string>("GamepadTopicName").getValue());
        emergencyStopTopicPrx = getTopic<EmergencyStopListenerPrx>(
            getProperty<std::string>("EmergencyStopTopicName").getValue());
 
        createGamepadTab();
        createEmergencyStopTab();
 
        RemoteGui_startRunningTask();
    }
 
    void
    InterventionDebugger::onDisconnectComponent()
    {
        gamepadTab.remove();
        estopTab.remove();
    }
 
    void
    InterventionDebugger::onExitComponent()
    {
    }
 
    void
    InterventionDebugger::RemoteGui_update()
    {
        updateGamepadTab();
        updateEmergencyStopTab();
    }
 
    // ---- Gamepad Tab ----
 
    void
    InterventionDebugger::createGamepadTab()
    {
        VBoxLayout root;
 
        // --- Analog sticks ---
        {
            GroupBox group;
            group.setLabel("Analog Sticks");
 
            VBoxLayout layout;
 
            gamepadTab.leftStickX.setRange(-1.0f, 1.0f);
            gamepadTab.leftStickX.setSteps(200);
            gamepadTab.leftStickX.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("Left Stick X:"), gamepadTab.leftStickX});
 
            gamepadTab.leftStickY.setRange(-1.0f, 1.0f);
            gamepadTab.leftStickY.setSteps(200);
            gamepadTab.leftStickY.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("Left Stick Y:"), gamepadTab.leftStickY});
 
            gamepadTab.rightStickX.setRange(-1.0f, 1.0f);
            gamepadTab.rightStickX.setSteps(200);
            gamepadTab.rightStickX.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("Right Stick X:"), gamepadTab.rightStickX});
 
            gamepadTab.rightStickY.setRange(-1.0f, 1.0f);
            gamepadTab.rightStickY.setSteps(200);
            gamepadTab.rightStickY.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("Right Stick Y:"), gamepadTab.rightStickY});
 
            group.addChild(layout);
            root.addChild(group);
        }
 
        // --- D-Pad ---
        {
            GroupBox group;
            group.setLabel("D-Pad");
 
            VBoxLayout layout;
 
            gamepadTab.dPadX.setRange(-1.0f, 1.0f);
            gamepadTab.dPadX.setSteps(200);
            gamepadTab.dPadX.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("D-Pad X:"), gamepadTab.dPadX});
 
            gamepadTab.dPadY.setRange(-1.0f, 1.0f);
            gamepadTab.dPadY.setSteps(200);
            gamepadTab.dPadY.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("D-Pad Y:"), gamepadTab.dPadY});
 
            group.addChild(layout);
            root.addChild(group);
        }
 
        // --- Triggers ---
        {
            GroupBox group;
            group.setLabel("Triggers");
 
            VBoxLayout layout;
 
            gamepadTab.leftTrigger.setRange(0.0f, 1.0f);
            gamepadTab.leftTrigger.setSteps(100);
            gamepadTab.leftTrigger.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("Left Trigger:"), gamepadTab.leftTrigger});
 
            gamepadTab.rightTrigger.setRange(0.0f, 1.0f);
            gamepadTab.rightTrigger.setSteps(100);
            gamepadTab.rightTrigger.setValue(0.0f);
            layout.addChild(HBoxLayout{Label("Right Trigger:"), gamepadTab.rightTrigger});
 
            group.addChild(layout);
            root.addChild(group);
        }
 
        // --- Buttons ---
        {
            GroupBox group;
            group.setLabel("Buttons");
 
            VBoxLayout layout;
 
            gamepadTab.aButton.setLabel("A");
            gamepadTab.aButton.setValue(false);
            gamepadTab.bButton.setLabel("B");
            gamepadTab.bButton.setValue(false);
            gamepadTab.xButton.setLabel("X");
            gamepadTab.xButton.setValue(false);
            gamepadTab.yButton.setLabel("Y");
            gamepadTab.yButton.setValue(false);
            layout.addChild(HBoxLayout{
                Label("Face:"),
                gamepadTab.aButton,
                gamepadTab.bButton,
                gamepadTab.xButton,
                gamepadTab.yButton});
 
            gamepadTab.leftButton.setLabel("LB");
            gamepadTab.leftButton.setValue(false);
            gamepadTab.rightButton.setLabel("RB");
            gamepadTab.rightButton.setValue(false);
            layout.addChild(
                HBoxLayout{Label("Bumpers:"), gamepadTab.leftButton, gamepadTab.rightButton});
 
            gamepadTab.backButton.setLabel("Back");
            gamepadTab.backButton.setValue(false);
            gamepadTab.startButton.setLabel("Start");
            gamepadTab.startButton.setValue(false);
            gamepadTab.theMiddleButton.setLabel("Middle");
            gamepadTab.theMiddleButton.setValue(false);
            layout.addChild(HBoxLayout{Label("System:"),
                                        gamepadTab.backButton,
                                        gamepadTab.startButton,
                                        gamepadTab.theMiddleButton});
 
            gamepadTab.leftStickButton.setLabel("L3");
            gamepadTab.leftStickButton.setValue(false);
            gamepadTab.rightStickButton.setLabel("R3");
            gamepadTab.rightStickButton.setValue(false);
            layout.addChild(HBoxLayout{
                Label("Stick Buttons:"), gamepadTab.leftStickButton, gamepadTab.rightStickButton});
 
            group.addChild(layout);
            root.addChild(group);
        }
 
        // --- Reset ---
        {
            gamepadTab.resetAll.setLabel("Reset All to Zero");
            root.addChild(HBoxLayout{gamepadTab.resetAll, HSpacer()});
        }
 
        root.addChild(VSpacer());
 
        RemoteGui_createTab("Gamepad", root, &gamepadTab);
    }
 
    void
    InterventionDebugger::updateGamepadTab()
    {
        // Note: receiveUpdates()/sendUpdates() are called automatically by the
        // LightweightRemoteGui framework around RemoteGui_update().
 
        if (gamepadTab.resetAll.wasClicked())
        {
            gamepadTab.leftStickX.setValue(0.0f);
            gamepadTab.leftStickY.setValue(0.0f);
            gamepadTab.rightStickX.setValue(0.0f);
            gamepadTab.rightStickY.setValue(0.0f);
            gamepadTab.dPadX.setValue(0.0f);
            gamepadTab.dPadY.setValue(0.0f);
            gamepadTab.leftTrigger.setValue(0.0f);
            gamepadTab.rightTrigger.setValue(0.0f);
 
            gamepadTab.aButton.setValue(false);
            gamepadTab.bButton.setValue(false);
            gamepadTab.xButton.setValue(false);
            gamepadTab.yButton.setValue(false);
            gamepadTab.leftButton.setValue(false);
            gamepadTab.rightButton.setValue(false);
            gamepadTab.backButton.setValue(false);
            gamepadTab.startButton.setValue(false);
            gamepadTab.theMiddleButton.setValue(false);
            gamepadTab.leftStickButton.setValue(false);
            gamepadTab.rightStickButton.setValue(false);
        }
 
        publishGamepadState();
    }
 
    void
    InterventionDebugger::publishGamepadState()
    {
        GamepadData data;
        data.leftStickX = gamepadTab.leftStickX.getValue();
        data.leftStickY = gamepadTab.leftStickY.getValue();
        data.rightStickX = gamepadTab.rightStickX.getValue();
        data.rightStickY = gamepadTab.rightStickY.getValue();
        data.dPadX = gamepadTab.dPadX.getValue();
        data.dPadY = gamepadTab.dPadY.getValue();
        data.leftTrigger = gamepadTab.leftTrigger.getValue();
        data.rightTrigger = gamepadTab.rightTrigger.getValue();
 
        data.aButton = gamepadTab.aButton.getValue();
        data.bButton = gamepadTab.bButton.getValue();
        data.xButton = gamepadTab.xButton.getValue();
        data.yButton = gamepadTab.yButton.getValue();
        data.leftButton = gamepadTab.leftButton.getValue();
        data.rightButton = gamepadTab.rightButton.getValue();
        data.backButton = gamepadTab.backButton.getValue();
        data.startButton = gamepadTab.startButton.getValue();
        data.theMiddleButton = gamepadTab.theMiddleButton.getValue();
        data.leftStickButton = gamepadTab.leftStickButton.getValue();
        data.rightStickButton = gamepadTab.rightStickButton.getValue();
 
        auto timestamp = new TimestampVariant(IceUtil::Time::now());
 
        gamepadTopicPrx->reportGamepadState(deviceName, gamepadName, data, timestamp);
    }
 
    // ---- Emergency Stop Tab ----
 
    void
    InterventionDebugger::createEmergencyStopTab()
    {
        VBoxLayout root;
 
        GroupBox group;
        group.setLabel("Emergency Stop");
 
        estopTab.emergencyStopActive.setLabel("Emergency Stop Active");
        estopTab.emergencyStopActive.setValue(false);
 
        VBoxLayout layout;
        layout.addChild(estopTab.emergencyStopActive);
        layout.addChild(VSpacer());
 
        group.addChild(layout);
        root.addChild(group);
        root.addChild(VSpacer());
 
        RemoteGui_createTab("EmergencyStop", root, &estopTab);
    }
 
    void
    InterventionDebugger::updateEmergencyStopTab()
    {
        if (estopTab.emergencyStopActive.hasValueChanged())
        {
            const bool active = estopTab.emergencyStopActive.getValue();
            const auto state = active ? EmergencyStopState::eEmergencyStopActive
                                      : EmergencyStopState::eEmergencyStopInactive;
 
            publishEmergencyStopState(state);
            lastPublishedEstopState = state;
        }
    }
 
    void
    InterventionDebugger::publishEmergencyStopState(EmergencyStopState state)
    {
        ARMARX_INFO << "Publishing emergency stop state: "
                    << (state == EmergencyStopState::eEmergencyStopActive ? "ACTIVE" : "INACTIVE");
 
        emergencyStopTopicPrx->reportEmergencyStopState(state);
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(InterventionDebugger,
                                         InterventionDebugger::GetDefaultName());
} // namespace armarx