HumanRobotInteractionDebugger.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    VisionX::ArmarXObjects::human_robot_interaction_debugger
 * @author     Joana Plewnia ( joana dot plewnia at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "HumanRobotInteractionDebugger.h"
 
#include <optional>
#include <utility>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/armem/core/Time.h>
 
#include <VisionX/libraries/armem_human/types.h>
 
namespace visionx::components::human_robot_interaction_debugger
{
 
    const std::string HumanRobotInteractionDebugger::interaction_provider_name =
        "human_robot_interaction_debugger";
 
    armarx::PropertyDefinitionsPtr
    HumanRobotInteractionDebugger::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
        def->defineRequiredProperty<std::string>("robotName");
        return def;
    }
 
    void
    HumanRobotInteractionDebugger::onInitComponent()
    {
    }
 
    void
    HumanRobotInteractionDebugger::onConnectComponent()
    {
        running = true;
        properties.robotName = this->getProperty<std::string>("robotName");
 
        // Connect reader and writer
        interactionWriter.connect(memoryNameSystem());
        interactionReader.connect(memoryNameSystem());
 
        while (running)
        {
            this->run();
        }
    }
 
    void
    HumanRobotInteractionDebugger::onDisconnectComponent()
    {
    }
 
    void
    HumanRobotInteractionDebugger::onExitComponent()
    {
    }
 
    void
    HumanRobotInteractionDebugger::run()
    {
        ARMARX_INFO << "Use 'interaction [firstName] [lastName] [interactionType] "
                       "[interactionName] [robotName] [confidence]' to commit an interaction";
        ARMARX_INFO << "  Interaction types: HANDOVER, ANSWERING_QUESTION, SERVING_DRINK, "
                       "SERVING_FOOD, GUIDING, ASSISTING, CONVERSATION, GREETING, FAREWELL";
        ARMARX_INFO << "  Example: interaction John Doe HANDOVER HandoverBottle Armar6 0.95";
        ARMARX_INFO << "Use 'read' to read all interactions";
        ARMARX_INFO << "Use 'exit' to exit";
 
        std::string command;
        std::cin >> command;
 
        if (command == "interaction")
        {
            interactionCommand();
        }
        else if (command == "read")
        {
            readInteractionCommand();
        }
        else if (command == "exit")
        {
            running = false;
        }
        else
        {
            ARMARX_WARNING << "Unrecognized command. Ignoring... "
                           << "\n"
                           << command;
        }
 
        std::cin.clear();
    }
 
    void
    HumanRobotInteractionDebugger::interactionCommand()
    {
        std::string firstName, lastName, interactionTypeStr, interactionName, robotName,
            confidenceStr;
        std::cin >> firstName >> lastName >> interactionTypeStr >> interactionName >> robotName >>
            confidenceStr;
 
        float confidence = std::stof(confidenceStr);
 
        // Parse interaction type
        armarx::armem::human::InteractionType interactionType =
            armarx::armem::human::InteractionType::UNKNOWN;
 
        std::string interactionTypeUpper = simox::alg::to_upper(interactionTypeStr);
 
        if (interactionTypeUpper == "HANDOVER")
            interactionType = armarx::armem::human::InteractionType::HANDOVER;
        else if (interactionTypeUpper == "ANSWERING_QUESTION")
            interactionType = armarx::armem::human::InteractionType::ANSWERING_QUESTION;
        else if (interactionTypeUpper == "SERVING_DRINK")
            interactionType = armarx::armem::human::InteractionType::SERVING_DRINK;
        else if (interactionTypeUpper == "SERVING_FOOD")
            interactionType = armarx::armem::human::InteractionType::SERVING_FOOD;
        else if (interactionTypeUpper == "GUIDING")
            interactionType = armarx::armem::human::InteractionType::GUIDING;
        else if (interactionTypeUpper == "ASSISTING")
            interactionType = armarx::armem::human::InteractionType::ASSISTING;
        else if (interactionTypeUpper == "CONVERSATION")
            interactionType = armarx::armem::human::InteractionType::CONVERSATION;
        else if (interactionTypeUpper == "GREETING")
            interactionType = armarx::armem::human::InteractionType::GREETING;
        else if (interactionTypeUpper == "FAREWELL")
            interactionType = armarx::armem::human::InteractionType::FAREWELL;
        else if (interactionTypeUpper == "CUSTOM")
            interactionType = armarx::armem::human::InteractionType::CUSTOM;
        else
        {
            ARMARX_WARNING << "Unknown interaction type: " << interactionTypeStr
                           << ". Using UNKNOWN.";
        }
 
        commitInteraction(firstName, lastName, interactionType, interactionName, robotName, confidence);
    }
 
    void
    HumanRobotInteractionDebugger::readInteractionCommand()
    {
        // Use InteractionReader to query all interactions
        armarx::armem::human::client::InteractionReader::Query query = {
            .providerName = "", // Query all providers
            .timestamp = armarx::armem::Time::Now(),
            .maxAge = armarx::Duration::Minutes(1000)};
 
        auto result = interactionReader.query(query, true);
 
        if (not result)
        {
            ARMARX_WARNING << "Failed to read interactions from memory: " << result.errorMessage;
            return;
        }
 
        if (result.interactions.empty())
        {
            ARMARX_INFO << "No interactions found in memory.";
            return;
        }
 
        ARMARX_INFO << "Interactions in memory (" << result.interactions.size() << " total):";
        for (const auto& interaction : result.interactions)
        {
            ARMARX_INFO << "  Interaction: " << interaction.interactionName;
            ARMARX_INFO << "    Type: " << static_cast<int>(interaction.interactionType);
            ARMARX_INFO << "    Confidence: " << interaction.confidence;
 
            if (interaction.personID.has_value())
            {
                ARMARX_INFO << "    Person: " << interaction.personID.value().firstName << " "
                            << interaction.personID.value().lastName;
            }
            else
            {
                ARMARX_INFO << "    Person: (none)";
            }
 
            if (interaction.robotName.has_value())
            {
                ARMARX_INFO << "    Robot: " << interaction.robotName.value();
            }
            else
            {
                ARMARX_INFO << "    Robot: (none)";
            }
 
            if (interaction.description.has_value())
            {
                ARMARX_INFO << "    Description: " << interaction.description.value();
            }
 
            ARMARX_INFO << "    Start time: " << interaction.startTime;
            ARMARX_INFO << "    End time: " << interaction.endTime;
            ARMARX_INFO << ""; // Empty line for readability
        }
    }
 
    void
    HumanRobotInteractionDebugger::commitInteraction(
        const std::string& firstName,
        const std::string& lastName,
        armarx::armem::human::InteractionType interactionType,
        const std::string& interactionName,
        const std::string& robotName,
        float confidence)
    {
        // Create the HumanRobotInteraction business object
        armarx::armem::human::HumanRobotInteraction interaction;
        interaction.interactionType = interactionType;
        interaction.interactionName = interactionName;
        interaction.confidence = confidence;
        interaction.startTime = armarx::DateTime::Now();
        interaction.endTime = armarx::DateTime::Now();
        interaction.robotName = robotName;
 
        // Set person ID
        armarx::armem::human::PersonID personID;
        personID.firstName = firstName;
        personID.lastName = lastName;
        interaction.personID = personID;
 
        interaction.description = "Debug interaction created by HumanRobotInteractionDebugger";
 
        // Use InteractionWriter to commit
        std::string entityName = personID.toMemoryEntityID();
        ARMARX_INFO << "Committing interaction '" << interactionName << "' for person " << firstName
                    << " " << lastName << " with robot " << robotName << " (entity: " << entityName
                    << ") with confidence " << confidence;
 
        bool success = interactionWriter.commitHumanRobotInteraction(
            interaction, interaction_provider_name, armarx::armem::Time::Now());
 
        if (success)
        {
            ARMARX_INFO << "Successfully committed interaction.";
        }
        else
        {
            ARMARX_WARNING << "Failed to commit interaction.";
        }
    }
 
    std::string
    HumanRobotInteractionDebugger::getDefaultName() const
    {
        return "HumanRobotInteractionDebugger";
    }
 
} // namespace visionx::components::human_robot_interaction_debugger