SkillsMemory.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::SkillsMemory
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SkillsMemory.h"
 
#include <vector>
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/LocalTimeServer.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include "RobotAPI/libraries/armem_skills/server/segment/ComposedSkillLibrarySegment.h"
#include "RobotAPI/libraries/skills/manager/SkillManagerComponentPluginUser.h"
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
#include <RobotAPI/libraries/skills/core/aron/FluxioSkill.aron.generated.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(armarx::SkillsMemory, "SkillsMemory");
 
namespace armarx
{
 
    SkillsMemory::SkillsMemory() :
        ReadWritePluginUser(),
        StatechartListenerComponentPluginUser(),
        SkillManagerComponentPluginUser(),
        statechartListenerProviderSegment(iceAdapter()),
        executableSkillCoreSegment(iceAdapter()),
        composedSkillCoreSegment(iceAdapter()),
        profileCoreSegment(iceAdapter()),
        skillEventCoreSegment(iceAdapter()),
        skillExecutionRequestCoreSegment(iceAdapter())
    {
    }
 
    armarx::PropertyDefinitionsPtr
    SkillsMemory::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        const std::string prefix = "mem.";
        statechartListenerProviderSegment.defineProperties(defs, prefix + "statechartlistener.");
        executableSkillCoreSegment.defineProperties(defs, prefix + "executableskill.");
        composedSkillCoreSegment.defineProperties(defs, prefix + "composedskill.");
        profileCoreSegment.defineProperties(defs, prefix + "profile.");
        skillExecutionRequestCoreSegment.defineProperties(defs, prefix + "executionrequest.");
        skillEventCoreSegment.defineProperties(defs, prefix + "event.");
 
        setMemoryName(MemoryName);
 
        //Debug Observer:
        defs->topic(debugObserver);
        defs->optional(
            p.useDebugObserver, "useDebugObserver", "Enable debug observer timing measurements");
 
        return defs;
    }
 
    std::string
    SkillsMemory::getDefaultName() const
    {
        return "SkillMemory";
    }
 
    void
    SkillsMemory::onInitComponent()
    {
        statechartListenerProviderSegment.init();
        executableSkillCoreSegment.init();
        composedSkillCoreSegment.init();
        profileCoreSegment.init();
        skillExecutionRequestCoreSegment.init();
        skillEventCoreSegment.init();
 
        ARMARX_INFO << "Using debug observer: " << p.useDebugObserver;
    }
 
    void
    SkillsMemory::onConnectComponent()
    {
        // Pass the debug observer to the long-term memory for timing measurements
        if (p.useDebugObserver && debugObserver)
        {
            longtermMemory().setDebugObserver(debugObserver);
            ARMARX_INFO << "Debug observer set for LTM timing measurements";
        }
 
        // create RemoteGUI tab and processing thread for GUI updates
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
    }
 
    void
    SkillsMemory::onDisconnectComponent()
    {
    }
 
    void
    SkillsMemory::onExitComponent()
    {
    }
 
    armem::data::CommitResult
    SkillsMemory::commit(const armem::data::Commit& commit, const Ice::Current& current)
    {
        // This function is overloaded to check for skill executions
        // First pass arg to parent func to ensure that data is written into memory and that clients are notified
        IceUtil::Time startTime = IceUtil::Time::now();
 
        armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
 
        skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
        getProxy(myPrx, -1);
 
        for (const auto& up : commit.updates)
        {
            if (up.entityID.coreSegmentName ==
                skills::segment::SkillExecutionRequestCoreSegment::CoreSegmentName)
            {
                for (const auto& instance : up.instancesData)
                {
                    ARMARX_CHECK_NOT_NULL(instance);
 
                    // Safety net: convertCommit() and downstream conversions
                    // can throw on corrupted data, type mismatches, or unknown
                    // enum values. Without this catch, any such exception
                    // propagates out of the Ice servant call and crashes the
                    // entire SkillMemory component.
                    try
                    {
                        skills::SkillExecutionRequest exInfo =
                            skillExecutionRequestCoreSegment.convertCommit(instance);
                        SkillManagerComponentPluginUser::executeSkill(exInfo.toManagerIce(),
                                                                      current);
                    }
                    catch (const std::exception& e)
                    {
                        ARMARX_WARNING << "Failed to process skill execution commit: "
                                       << e.what()
                                       << ". The local memory commit still proceeds.";
                    }
                }
            }
        }
 
        if (p.useDebugObserver and debugObserver)
        {
            debugObserver->setDebugChannel(
                "SkillMemory",
                {
                    {"Memory | t commit [ms]",
                     new Variant((IceUtil::Time::now() - startTime).toMilliSecondsDouble())},
                });
        }
 
        return result;
    }
 
    void
    SkillsMemory::addProvider(const skills::manager::dto::ProviderInfo& info,
                              const Ice::Current& current)
    {
        ARMARX_INFO << "Adding provider to skill memory: " << info.providerId.providerName;
        SkillManagerComponentPluginUser::addProvider(info, current);
 
        // log skills to memory
        auto p = skills::ProviderInfo::FromIce(info);
        executableSkillCoreSegment.addSkillProvider(p);
    }
 
    void
    SkillsMemory::removeProvider(const skills::manager::dto::ProviderID& provider,
                                 const Ice::Current& current)
    {
        executableSkillCoreSegment.removeSkillProvider(provider.providerName);
 
        // remove skills from memory
        SkillManagerComponentPluginUser::removeProvider(provider, current);
    }
 
    skills::manager::dto::SkillStatusUpdate
    SkillsMemory::executeSkill(const skills::manager::dto::SkillExecutionRequest& info,
                               const Ice::Current& current)
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        auto e = skills::SkillExecutionRequest::FromIce(info);
        skillExecutionRequestCoreSegment.addSkillExecutionRequest(e);
 
        skills::manager::dto::SkillStatusUpdate result =
            SkillManagerComponentPluginUser::executeSkill(info, current);
 
        if (p.useDebugObserver and debugObserver)
        {
            debugObserver->setDebugChannel(
                "SkillMemory",
                {
                    {"Memory | t executeSkill [ms]",
                     new Variant((IceUtil::Time::now() - startTime).toMilliSecondsDouble())},
                });
        }
 
        return result;
    }
 
    skills::manager::dto::SkillExecutionID
    SkillsMemory::executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& info,
                                    const Ice::Current& current)
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        auto e = skills::SkillExecutionRequest::FromIce(info);
        skillExecutionRequestCoreSegment.addSkillExecutionRequest(e);
 
        skills::manager::dto::SkillExecutionID result =
            SkillManagerComponentPluginUser::executeSkillAsync(info, current);
 
        if (p.useDebugObserver and debugObserver)
        {
            debugObserver->setDebugChannel(
                "SkillMemory",
                {
                    {"Memory | t executeSkillAsync [ms]",
                     new Variant((IceUtil::Time::now() - startTime).toMilliSecondsDouble())},
                });
        }
 
        return result;
    }
 
    void
    SkillsMemory::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
                                       const skills::callback::dto::ProviderID& providerId,
                                       const Ice::Current& current)
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        {
            auto p = skills::ProviderID::FromIce(providerId);
            auto u = skills::SkillStatusUpdate::FromIce(update, p);
            skillEventCoreSegment.addSkillUpdateEvent(u);
        }
 
        if (p.useDebugObserver and debugObserver)
        {
            debugObserver->setDebugChannel(
                "SkillMemory",
                {
                    {"Memory | t updateStatusForSkill [ms]",
                     new Variant((IceUtil::Time::now() - startTime).toMilliSecondsDouble())},
                });
        }
    }
 
    IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
    SkillsMemory::getSkillExecutionStatus(const skills::manager::dto::SkillExecutionID& executionId,
                                          const Ice::Current& current)
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        auto eid = skills::SkillExecutionID::FromIce(executionId);
        auto op = this->skillEventCoreSegment.getSkillStatusUpdate(eid);
 
        if (p.useDebugObserver and debugObserver)
        {
            debugObserver->setDebugChannel(
                "SkillMemory",
                {
                    {"Memory | t getSkillStatusUpdate(eid) [ms]",
                     new Variant((IceUtil::Time::now() - startTime).toMilliSecondsDouble())},
                });
        }
 
        if (op.has_value())
        {
            return op->toManagerIce();
        }
        return {};
    }
 
    skills::manager::dto::SkillStatusUpdateMap
    SkillsMemory::getSkillExecutionStatuses(const Ice::Current& current)
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        skills::manager::dto::SkillStatusUpdateMap ret;
        auto updates = this->skillEventCoreSegment.getSkillStatusUpdates();
 
        if (p.useDebugObserver and debugObserver)
        {
            debugObserver->setDebugChannel(
                "SkillMemory",
                {
                    {"Memory | t getSkillStatusUpdates [ms]",
                     new Variant((IceUtil::Time::now() - startTime).toMilliSecondsDouble())},
                });
 
            startTime = IceUtil::Time::now();
        }
 
        for (const auto& [k, v] : updates)
        {
            ret.insert({k.toManagerIce(), v.toManagerIce()});
        }
 
        if (p.useDebugObserver and debugObserver)
        {
            debugObserver->setDebugChannel(
                "SkillMemory",
                {
                    {"Memory | t getSkillStatusUpdates.toManagerIce [ms]",
                     new Variant((IceUtil::Time::now() - startTime).toMilliSecondsDouble())},
                });
        }
 
        return ret;
    }
 
    /*
     * Statechart stuff
     */
    void
    SkillsMemory::reportStatechartTransitionWithParameters(
        const ProfilerStatechartTransitionWithParameters& x,
        const Ice::Current&)
    {
        statechartListenerProviderSegment.reportStatechartTransitionWithParameters(x);
    }
 
    void
    SkillsMemory::reportStatechartTransitionWithParametersList(
        const ProfilerStatechartTransitionWithParametersList& x,
        const Ice::Current&)
    {
        statechartListenerProviderSegment.reportStatechartTransitionWithParametersList(x);
    }
 
    // RemoteGUI
    void
    SkillsMemory::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        VBoxLayout root;
 
        tab.clearSkillEventsButton.setLabel("Clear Skill Events WM");
        root.addChild(tab.clearSkillEventsButton);
 
        tab.clearSkillExecutionRequestsButton.setLabel("Clear Skill Execution Requests WM");
        root.addChild(tab.clearSkillExecutionRequestsButton);
 
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    SkillsMemory::RemoteGui_update()
    {
        if (tab.clearSkillEventsButton.wasClicked())
        {
            clearSkillEventsWorkingMemory();
        }
        if (tab.clearSkillExecutionRequestsButton.wasClicked())
        {
            clearSkillExecutionRequestsWorkingMemory();
        }
    }
 
    void
    SkillsMemory::clearSkillEventsWorkingMemory()
    {
        bool consolidated = false;
        const std::size_t snapshotCount = skillEventCoreSegment.clearAndConsolidate(
            longtermMemory(), iceAdapter().workingMemory->name(), consolidated);
 
        ARMARX_IMPORTANT << "Cleared SkillEvent working memory: removed " << snapshotCount
                         << " snapshot(s). Consolidated to LTM: "
                         << (consolidated ? "yes" : "no (recording disabled)") << ".";
    }
 
    void
    SkillsMemory::clearSkillExecutionRequestsWorkingMemory()
    {
        bool consolidated = false;
        const std::size_t snapshotCount = skillExecutionRequestCoreSegment.clearAndConsolidate(
            longtermMemory(), iceAdapter().workingMemory->name(), consolidated);
 
        ARMARX_IMPORTANT << "Cleared SkillExecutionRequest working memory: removed "
                         << snapshotCount << " snapshot(s). Consolidated to LTM: "
                         << (consolidated ? "yes" : "no (recording disabled)") << ".";
    }
 
    //****************************//
    //** Fluxio related methods **//
    //****************************//
 
    void
    SkillsMemory::saveSkill(const skills::manager::arondto::FluxioSkill& skill)
    {
        composedSkillCoreSegment.addSkill(skill);
    }
 
    std::optional<std::vector<skills::manager::arondto::FluxioSkill>>
    SkillsMemory::loadCompositeSkills()
    {
        return composedSkillCoreSegment.getSkills();
    }
 
    std::optional<std::vector<skills::manager::arondto::FluxioSkill>>
    SkillsMemory::loadCompositeSkillsOfProvider(const std::string& providerId)
    {
        return composedSkillCoreSegment.getSkillsOfProvider(providerId);
    }
 
    void
    SkillsMemory::addProfile(const skills::manager::arondto::FluxioProfile& profile)
    {
        profileCoreSegment.addProfile(profile);
    }
 
    std::optional<std::vector<skills::manager::arondto::FluxioProfile>>
    SkillsMemory::loadProfiles()
    {
        return profileCoreSegment.getProfiles();
    }
} // namespace armarx