TaskOutcomeDebugger.cpp

Go to the documentation of this file.

#include "TaskOutcomeDebugger.h"
 
#include <iostream>
#include <sstream>
#include <vector>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/armem/core/Time.h>
 
namespace armarx::components::task_outcome_debugger
{
 
    const std::string TaskOutcomeDebugger::providerName = "task_outcome_debugger";
 
    armarx::PropertyDefinitionsPtr
    TaskOutcomeDebugger::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
        return def;
    }
 
    void
    TaskOutcomeDebugger::onInitComponent()
    {
    }
 
    void
    TaskOutcomeDebugger::onConnectComponent()
    {
        running = true;
 
        writer.connect(memoryNameSystem());
        reader.connect(memoryNameSystem());
 
        while (running)
        {
            this->run();
        }
    }
 
    void
    TaskOutcomeDebugger::onDisconnectComponent()
    {
    }
 
    void
    TaskOutcomeDebugger::onExitComponent()
    {
    }
 
    void
    TaskOutcomeDebugger::run()
    {
        ARMARX_INFO << "\n--- TaskOutcome Debugger ---";
        ARMARX_INFO << "Commands:";
        ARMARX_INFO << "  commit <taskName> <taskType> <outcome> [errorMessage]";
        ARMARX_INFO << "    taskType: GRASPING_FAMILIAR, GRASPING_KNOWN, MANIPULATION,";
        ARMARX_INFO << "             VERBALIZATION, DETECTION, NAVIGATION, SPEAKING,";
        ARMARX_INFO << "             SPECIAL, UNKNOWN";
        ARMARX_INFO << "    outcome:  SUCCESS, FAILURE, ABORTED, SUSPENDED, UNKNOWN";
        ARMARX_INFO << "    Example: commit grasp_cup GRASPING_FAMILIAR SUCCESS";
        ARMARX_INFO << "    Example: commit grasp_cup GRASPING_KNOWN FAILURE gripper_timeout";
        ARMARX_INFO << "  read                 - read all task outcomes";
        ARMARX_INFO << "  read_type <taskType>  - read outcomes by task type";
        ARMARX_INFO << "  read_failures         - read only failures";
        ARMARX_INFO << "  exit                  - exit the debugger";
 
        std::string command;
        std::cin >> command;
 
        if (command == "commit")
        {
            commitCommand();
        }
        else if (command == "read")
        {
            readAllCommand();
        }
        else if (command == "read_type")
        {
            readByTypeCommand();
        }
        else if (command == "read_failures")
        {
            readFailuresCommand();
        }
        else if (command == "exit")
        {
            running = false;
        }
        else
        {
            ARMARX_WARNING << "Unrecognized command: " << command;
        }
 
        std::cin.clear();
    }
 
    void
    TaskOutcomeDebugger::commitCommand()
    {
        // Read the rest of the line to handle optional errorMessage
        std::string restOfLine;
        std::getline(std::cin, restOfLine);
 
        std::istringstream iss(restOfLine);
        std::string taskName, taskTypeStr, outcomeStr;
 
        iss >> taskName >> taskTypeStr >> outcomeStr;
 
        if (taskName.empty() || taskTypeStr.empty() || outcomeStr.empty())
        {
            ARMARX_WARNING
                << "Usage: commit <taskName> <taskType> <outcome> [errorMessage]";
            return;
        }
 
        auto taskType = parseTaskType(taskTypeStr);
        auto outcomeType = parseOutcomeType(outcomeStr);
 
        if (outcomeType == armem::task_outcome::TaskOutcomeType::UNKNOWN &&
            simox::alg::to_upper(outcomeStr) != "UNKNOWN")
        {
            ARMARX_WARNING << "Unknown outcome type: " << outcomeStr << ". Using UNKNOWN.";
        }
 
        if (taskType == armem::task_outcome::TaskType::UNKNOWN &&
            simox::alg::to_upper(taskTypeStr) != "UNKNOWN")
        {
            ARMARX_WARNING << "Unknown task type: " << taskTypeStr << ". Using UNKNOWN.";
        }
 
        // Read optional error message (rest of line)
        std::optional<std::string> errorMsg;
        std::string remaining;
        if (std::getline(iss >> std::ws, remaining) && !remaining.empty())
        {
            errorMsg = remaining;
        }
 
        commitOutcome(taskName, taskType, outcomeType, errorMsg);
    }
 
    void
    TaskOutcomeDebugger::readAllCommand()
    {
        armem::task_outcome::client::TaskOutcomeReader::Query query = {
            .providerName = "",
            .taskTypeFilter = std::nullopt,
            .outcomeFilter = std::nullopt,
            .timestamp = armarx::armem::Time::Now(),
            .maxAge = armarx::Duration::Minutes(1000)};
 
        auto result = reader.query(query);
 
        if (not result)
        {
            ARMARX_WARNING << "Failed to read task outcomes: " << result.errorMessage;
            return;
        }
 
        if (result.outcomes.empty())
        {
            ARMARX_INFO << "No task outcomes found in memory.";
            return;
        }
 
        ARMARX_INFO << "Task outcomes in memory (" << result.outcomes.size() << " total):";
        printOutcomes(result.outcomes);
    }
 
    void
    TaskOutcomeDebugger::readByTypeCommand()
    {
        std::string taskTypeStr;
        std::cin >> taskTypeStr;
 
        if (taskTypeStr.empty())
        {
            ARMARX_WARNING << "Usage: read_type <taskType>";
            return;
        }
 
        auto taskType = parseTaskType(taskTypeStr);
 
        auto outcomes =
            reader.queryByTaskType(taskType, armarx::Duration::Minutes(1000));
 
        if (outcomes.empty())
        {
            ARMARX_INFO << "No task outcomes found for type '"
                        << taskTypeToString(taskType) << "'.";
            return;
        }
 
        ARMARX_INFO << "Task outcomes of type '" << taskTypeToString(taskType) << "' ("
                    << outcomes.size() << " total):";
        printOutcomes(outcomes);
    }
 
    void
    TaskOutcomeDebugger::readFailuresCommand()
    {
        auto outcomes = reader.queryFailures(armarx::Duration::Minutes(1000));
 
        if (outcomes.empty())
        {
            ARMARX_INFO << "No failures found in memory.";
            return;
        }
 
        ARMARX_INFO << "Failures in memory (" << outcomes.size() << " total):";
        printOutcomes(outcomes);
    }
 
    void
    TaskOutcomeDebugger::commitOutcome(
        const std::string& taskName,
        armem::task_outcome::TaskType taskType,
        armem::task_outcome::TaskOutcomeType outcomeType,
        const std::optional<std::string>& errorMessage)
    {
        armem::task_outcome::TaskOutcome outcome;
        outcome.taskName = taskName;
        outcome.taskType = taskType;
        outcome.outcomeType = outcomeType;
        outcome.agent = "ARMAR-7";
        outcome.errorMessage = errorMessage;
        outcome.startTime = armarx::DateTime::Now();
        outcome.endTime = armarx::DateTime::Now();
 
        // Fill specialTaskTypeDescription for SPECIAL type
        if (taskType == armem::task_outcome::TaskType::SPECIAL)
        {
            outcome.specialTaskTypeDescription = "custom_debug_task";
        }
 
        // Add fake failureInfo for FAILURE outcomes
        if (outcomeType == armem::task_outcome::TaskOutcomeType::FAILURE)
        {
            armem::task_outcome::FailureInfo fi;
            fi.failureType.execution.robotNotMoving = true;
            fi.specialFailureDescription = std::nullopt;
            outcome.failureInfo = fi;
        }
 
        // Add fake context with required skillName, parameters, and additional entries
        outcome.context.skillName = "GraspObject";
        outcome.context.parameters = {{"object", "cup_01"},
                                      {"approach_direction", "left"},
                                      {"gripper", "right_hand"}};
        outcome.context.additional = {{"location", "kitchen_table"},
                                      {"attempt_number", "3"},
                                      {"planner", "grasp_planner_v2"}};
        outcome.context.naturalLanguageDescription = "Grasp the cup from the kitchen table using the right hand";
        outcome.context.taskPreconditions["object_visible"] = true;
        outcome.context.taskPreconditions["target_object"] = std::string("cup_01");
        outcome.context.taskPreconditions["reachable_objects"] =
            std::vector<std::string>{"cup_01", "plate_02", "bowl_03"};
        outcome.context.taskPreconditions["min_grip_force"] = 2.5;
 
        outcome.context.taskPostconditions["object_in_hand"] = true;
        outcome.context.taskPostconditions["placement_surface"] = std::string("kitchen_table");
        outcome.context.taskPostconditions["contacted_objects"] =
            std::vector<std::string>{"cup_01"};
        outcome.context.taskPostconditions["grip_force_applied"] = 3.7;
 
        ARMARX_INFO << "Committing task outcome: taskName='" << taskName
                    << "' taskType=" << taskTypeToString(taskType)
                    << " outcome=" << outcomeTypeToString(outcomeType)
                    << " agent='" << outcome.agent << "'"
                    << " skill='" << outcome.context.skillName << "'"
                    << (errorMessage.has_value()
                            ? " error='" + errorMessage.value() + "'"
                            : "");
 
        bool success =
            writer.commitTaskOutcome(outcome, providerName, armarx::armem::Time::Now());
 
        if (success)
        {
            ARMARX_INFO << "Successfully committed task outcome.";
        }
        else
        {
            ARMARX_WARNING << "Failed to commit task outcome.";
        }
    }
 
    void
    TaskOutcomeDebugger::printOutcomes(
        const std::vector<armem::task_outcome::TaskOutcome>& outcomes)
    {
        for (const auto& outcome : outcomes)
        {
            ARMARX_INFO << "  Task: " << outcome.taskName;
            ARMARX_INFO << "    Type: " << taskTypeToString(outcome.taskType);
            ARMARX_INFO << "    Outcome: " << outcomeTypeToString(outcome.outcomeType);
            ARMARX_INFO << "    Agent: " << outcome.agent;
 
            if (outcome.specialTaskTypeDescription.has_value())
            {
                ARMARX_INFO << "    Special type: "
                            << outcome.specialTaskTypeDescription.value();
            }
 
            if (outcome.errorMessage.has_value())
            {
                ARMARX_INFO << "    Error: " << outcome.errorMessage.value();
            }
 
            if (outcome.failureInfo.has_value())
            {
                ARMARX_INFO << "    Failure type: "
                            << failureTypeToString(outcome.failureInfo->failureType);
                if (outcome.failureInfo->specialFailureDescription.has_value())
                {
                    ARMARX_INFO << "    Failure description: "
                                << outcome.failureInfo->specialFailureDescription.value();
                }
            }
 
            ARMARX_INFO << "    Start time: " << outcome.startTime;
            ARMARX_INFO << "    End time: " << outcome.endTime;
 
            if (outcome.couldRecover.has_value())
            {
                ARMARX_INFO << "    Could recover: "
                            << (outcome.couldRecover.value() ? "true" : "false");
            }
 
            if (outcome.recoveryInfo.has_value())
            {
                ARMARX_INFO << "    Recovery info:";
                ARMARX_INFO << "      Time: " << outcome.recoveryInfo->recoveryTime;
                ARMARX_INFO << "      Measure: " << outcome.recoveryInfo->recoveryMeasure;
            }
 
            ARMARX_INFO << "    Context:";
            ARMARX_INFO << "      skillName = " << outcome.context.skillName;
            if (outcome.context.naturalLanguageDescription.has_value())
            {
                ARMARX_INFO << "      Description: "
                            << outcome.context.naturalLanguageDescription.value();
            }
            if (not outcome.context.parameters.empty())
            {
                ARMARX_INFO << "      Parameters:";
                for (const auto& [key, value] : outcome.context.parameters)
                {
                    ARMARX_INFO << "        " << key << " = " << value;
                }
            }
            if (not outcome.context.additional.empty())
            {
                ARMARX_INFO << "      Additional:";
                for (const auto& [key, value] : outcome.context.additional)
                {
                    ARMARX_INFO << "        " << key << " = " << value;
                }
            }
            if (not outcome.context.taskPreconditions.empty())
            {
                ARMARX_INFO << "      Preconditions:";
                for (const auto& [key, value] : outcome.context.taskPreconditions.items())
                {
                    ARMARX_INFO << "        " << key << " = " << value.dump();
                }
            }
            if (not outcome.context.taskPostconditions.empty())
            {
                ARMARX_INFO << "      Postconditions:";
                for (const auto& [key, value] : outcome.context.taskPostconditions.items())
                {
                    ARMARX_INFO << "        " << key << " = " << value.dump();
                }
            }
 
            ARMARX_INFO << "";
        }
    }
 
    armem::task_outcome::TaskOutcomeType
    TaskOutcomeDebugger::parseOutcomeType(const std::string& str)
    {
        std::string upper = simox::alg::to_upper(str);
 
        if (upper == "SUCCESS")
            return armem::task_outcome::TaskOutcomeType::SUCCESS;
        if (upper == "FAILURE")
            return armem::task_outcome::TaskOutcomeType::FAILURE;
        if (upper == "ABORTED")
            return armem::task_outcome::TaskOutcomeType::ABORTED;
        if (upper == "SUSPENDED")
            return armem::task_outcome::TaskOutcomeType::SUSPENDED;
 
        return armem::task_outcome::TaskOutcomeType::UNKNOWN;
    }
 
    armem::task_outcome::TaskType
    TaskOutcomeDebugger::parseTaskType(const std::string& str)
    {
        std::string upper = simox::alg::to_upper(str);
 
        if (upper == "GRASPING_FAMILIAR")
            return armem::task_outcome::TaskType::GRASPING_FAMILIAR;
        if (upper == "GRASPING_KNOWN")
            return armem::task_outcome::TaskType::GRASPING_KNOWN;
        if (upper == "MANIPULATION")
            return armem::task_outcome::TaskType::MANIPULATION;
        if (upper == "VERBALIZATION")
            return armem::task_outcome::TaskType::VERBALIZATION;
        if (upper == "DETECTION")
            return armem::task_outcome::TaskType::DETECTION;
        if (upper == "NAVIGATION")
            return armem::task_outcome::TaskType::NAVIGATION;
        if (upper == "SPEAKING")
            return armem::task_outcome::TaskType::SPEAKING;
        if (upper == "SPECIAL")
            return armem::task_outcome::TaskType::SPECIAL;
 
        return armem::task_outcome::TaskType::UNKNOWN;
    }
 
    std::string
    TaskOutcomeDebugger::outcomeTypeToString(armem::task_outcome::TaskOutcomeType type)
    {
        switch (type)
        {
            case armem::task_outcome::TaskOutcomeType::SUCCESS:
                return "SUCCESS";
            case armem::task_outcome::TaskOutcomeType::FAILURE:
                return "FAILURE";
            case armem::task_outcome::TaskOutcomeType::ABORTED:
                return "ABORTED";
            case armem::task_outcome::TaskOutcomeType::SUSPENDED:
                return "SUSPENDED";
            case armem::task_outcome::TaskOutcomeType::UNKNOWN:
                return "UNKNOWN";
        }
        return "UNKNOWN";
    }
 
    std::string
    TaskOutcomeDebugger::taskTypeToString(armem::task_outcome::TaskType type)
    {
        switch (type)
        {
            case armem::task_outcome::TaskType::GRASPING_FAMILIAR:
                return "GRASPING_FAMILIAR";
            case armem::task_outcome::TaskType::GRASPING_KNOWN:
                return "GRASPING_KNOWN";
            case armem::task_outcome::TaskType::MANIPULATION:
                return "MANIPULATION";
            case armem::task_outcome::TaskType::VERBALIZATION:
                return "VERBALIZATION";
            case armem::task_outcome::TaskType::DETECTION:
                return "DETECTION";
            case armem::task_outcome::TaskType::NAVIGATION:
                return "NAVIGATION";
            case armem::task_outcome::TaskType::SPEAKING:
                return "SPEAKING";
            case armem::task_outcome::TaskType::SPECIAL:
                return "SPECIAL";
            case armem::task_outcome::TaskType::UNKNOWN:
                return "UNKNOWN";
        }
        return "UNKNOWN";
    }
 
    std::string
    TaskOutcomeDebugger::failureTypeToString(const armem::task_outcome::FailureType& type)
    {
        std::vector<std::string> active;
 
        if (type.planning.noTrajectoryFound)
            active.push_back("Planning: NoTrajectoryFound");
        if (type.planning.noGraspFound)
            active.push_back("Planning: NoGraspFound");
        if (type.planning.special)
            active.push_back("Planning: Special");
 
        if (type.knowledge.perception.objectNotDetected)
            active.push_back("Knowledge/Perception: ObjectNotDetected");
        if (type.knowledge.perception.cameraNotWorking)
            active.push_back("Knowledge/Perception: CameraNotWorking");
        if (type.knowledge.knowledgeRetrieval.technicalFailure)
            active.push_back("Knowledge/Retrieval: TechnicalFailure");
        if (type.knowledge.knowledgeRetrieval.knowledgeNotInMemory)
            active.push_back("Knowledge/Retrieval: KnowledgeNotInMemory");
 
        if (type.execution.robotNotMoving)
            active.push_back("Execution: RobotNotMoving");
        if (type.execution.handNotClosing)
            active.push_back("Execution: HandNotClosing");
        if (type.execution.graspingFailureDetected)
            active.push_back("Execution: GraspingFailureDetected");
 
        if (type.interaction.noHumanForHandover)
            active.push_back("Interaction: NoHumanForHandover");
        if (type.interaction.desiredObjectNotAtLocation)
            active.push_back("Interaction: DesiredObjectNotAtLocation");
        if (type.interaction.communicationFailure)
            active.push_back("Interaction: CommunicationFailure");
 
        if (type.interruption.userStopped)
            active.push_back("Interruption: UserStopped");
 
        if (type.subskillFailure.failed)
            active.push_back("SubskillFailure: " + type.subskillFailure.subskillName);
 
        if (active.empty())
        {
            return "(none)";
        }
 
        std::string result;
        for (size_t i = 0; i < active.size(); ++i)
        {
            if (i > 0)
                result += ", ";
            result += active[i];
        }
        return result;
    }
 
    std::string
    TaskOutcomeDebugger::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    TaskOutcomeDebugger::GetDefaultName()
    {
        return "TaskOutcomeDebugger";
    }
 
} // namespace armarx::components::task_outcome_debugger