HumanActivityDebugger.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_activity_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 "HumanActivityDebugger.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_activity_debugger
{
 
    const std::string HumanActivityDebugger::activity_provider_name = "human_activity_debugger";
 
    armarx::PropertyDefinitionsPtr
    HumanActivityDebugger::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
        def->defineRequiredProperty<std::string>("robotName");
        return def;
    }
 
    void
    HumanActivityDebugger::onInitComponent()
    {
    }
 
    void
    HumanActivityDebugger::onConnectComponent()
    {
        running = true;
        properties.robotName = this->getProperty<std::string>("robotName");
 
        // Connect reader and writer
        activityWriter.connect(memoryNameSystem());
        activityReader.connect(memoryNameSystem());
 
        while (running)
        {
            this->run();
        }
    }
 
    void
    HumanActivityDebugger::onDisconnectComponent()
    {
    }
 
    void
    HumanActivityDebugger::onExitComponent()
    {
    }
 
    void
    HumanActivityDebugger::run()
    {
        ARMARX_INFO << "Use 'activity [firstName] [lastName] [activityType] [activityName] "
                       "[confidence]' to commit an activity";
        ARMARX_INFO << "  Activity types: WALKING, STANDING, SITTING, REACHING, GRASPING, "
                       "POINTING, WAVING, TALKING, LISTENING, WORKING, EATING, DRINKING";
        ARMARX_INFO << "  Example: activity John Doe WALKING walking 0.95";
        ARMARX_INFO << "Use 'read' to read all activities";
        ARMARX_INFO << "Use 'exit' to exit";
 
        std::string command;
        std::cin >> command;
 
        if (command == "activity")
        {
            activityCommand();
        }
        else if (command == "read")
        {
            readActivityCommand();
        }
        else if (command == "exit")
        {
            running = false;
        }
        else
        {
            ARMARX_WARNING << "Unrecognized command. Ignoring... "
                           << "\n"
                           << command;
        }
 
        std::cin.clear();
    }
 
    void
    HumanActivityDebugger::activityCommand()
    {
        std::string firstName, lastName, activityTypeStr, activityName, confidenceStr;
        std::cin >> firstName >> lastName >> activityTypeStr >> activityName >> confidenceStr;
 
        float confidence = std::stof(confidenceStr);
 
        // Parse activity type
        armarx::armem::human::ActivityType activityType =
            armarx::armem::human::ActivityType::UNKNOWN;
 
        std::string activityTypeUpper = simox::alg::to_upper(activityTypeStr);
 
        if (activityTypeUpper == "WALKING")
            activityType = armarx::armem::human::ActivityType::WALKING;
        else if (activityTypeUpper == "STANDING")
            activityType = armarx::armem::human::ActivityType::STANDING;
        else if (activityTypeUpper == "SITTING")
            activityType = armarx::armem::human::ActivityType::SITTING;
        else if (activityTypeUpper == "REACHING")
            activityType = armarx::armem::human::ActivityType::REACHING;
        else if (activityTypeUpper == "GRASPING")
            activityType = armarx::armem::human::ActivityType::GRASPING;
        else if (activityTypeUpper == "POINTING")
            activityType = armarx::armem::human::ActivityType::POINTING;
        else if (activityTypeUpper == "WAVING")
            activityType = armarx::armem::human::ActivityType::WAVING;
        else if (activityTypeUpper == "TALKING")
            activityType = armarx::armem::human::ActivityType::TALKING;
        else if (activityTypeUpper == "LISTENING")
            activityType = armarx::armem::human::ActivityType::LISTENING;
        else if (activityTypeUpper == "WORKING")
            activityType = armarx::armem::human::ActivityType::WORKING;
        else if (activityTypeUpper == "EATING")
            activityType = armarx::armem::human::ActivityType::EATING;
        else if (activityTypeUpper == "DRINKING")
            activityType = armarx::armem::human::ActivityType::DRINKING;
        else if (activityTypeUpper == "CUSTOM")
            activityType = armarx::armem::human::ActivityType::CUSTOM;
        else
        {
            ARMARX_WARNING << "Unknown activity type: " << activityTypeStr << ". Using UNKNOWN.";
        }
 
        commitActivity(firstName, lastName, activityType, activityName, confidence);
    }
 
    void
    HumanActivityDebugger::readActivityCommand()
    {
        // Use ActivityReader to query all activities
        armarx::armem::human::client::ActivityReader::Query query = {
            .providerName = "", // Query all providers
            .timestamp = armarx::armem::Time::Now(),
            .maxAge = armarx::Duration::Minutes(1000)};
 
        auto result = activityReader.query(query, true);
 
        if (not result)
        {
            ARMARX_WARNING << "Failed to read activities from memory: " << result.errorMessage;
            return;
        }
 
        if (result.activities.empty())
        {
            ARMARX_INFO << "No activities found in memory.";
            return;
        }
 
        ARMARX_INFO << "Activities in memory (" << result.activities.size() << " total):";
        for (const auto& activity : result.activities)
        {
            ARMARX_INFO << "  Activity: " << activity.activityName;
            ARMARX_INFO << "    Type: " << static_cast<int>(activity.activityType);
            ARMARX_INFO << "    Confidence: " << activity.confidence;
 
            if (activity.personID.has_value())
            {
                ARMARX_INFO << "    Person: " << activity.personID.value().firstName << " "
                            << activity.personID.value().lastName;
            }
            else
            {
                ARMARX_INFO << "    Person: (none)";
            }
 
            if (activity.description.has_value())
            {
                ARMARX_INFO << "    Description: " << activity.description.value();
            }
 
            ARMARX_INFO << "    Start time: " << activity.startTime;
            ARMARX_INFO << "    End time: " << activity.endTime;
            ARMARX_INFO << ""; // Empty line for readability
        }
    }
 
    void
    HumanActivityDebugger::commitActivity(const std::string& firstName,
                                          const std::string& lastName,
                                          armarx::armem::human::ActivityType activityType,
                                          const std::string& activityName,
                                          float confidence)
    {
        // Create the HumanActivity business object
        armarx::armem::human::HumanActivity activity;
        activity.activityType = activityType;
        activity.activityName = activityName;
        activity.confidence = confidence;
        activity.startTime = armarx::DateTime::Now();
        activity.endTime = armarx::DateTime::Now();
 
        // Set person ID
        armarx::armem::human::PersonID personID;
        personID.firstName = firstName;
        personID.lastName = lastName;
        activity.personID = personID;
 
        activity.description = "Debug activity created by HumanActivityDebugger";
 
        // Use ActivityWriter to commit
        std::string entityName = personID.toMemoryEntityID();
        ARMARX_INFO << "Committing activity '" << activityName << "' for person " << firstName
                    << " " << lastName << " (entity: " << entityName << ") with confidence "
                    << confidence;
 
        bool success = activityWriter.commitHumanActivity(
            activity, activity_provider_name, armarx::armem::Time::Now());
 
        if (success)
        {
            ARMARX_INFO << "Successfully committed activity.";
        }
        else
        {
            ARMARX_WARNING << "Failed to commit activity.";
        }
    }
 
    std::string
    HumanActivityDebugger::getDefaultName() const
    {
        return "HumanActivityDebugger";
    }
 
} // namespace visionx::components::human_activity_debugger