SkillProviderComponentPlugin.cpp

Go to the documentation of this file.

#include "SkillProviderComponentPlugin.h"
 
#include <chrono>
#include <exception>
#include <experimental/memory>
#include <map>
#include <memory>
#include <mutex>
#include <optional>
#include <shared_mutex>
#include <sstream>
#include <string>
#include <thread>
#include <tuple>
#include <utility>
 
#include <Ice/Current.h>
#include <IceUtil/Optional.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
#include <RobotAPI/libraries/skills/core/ProviderID.h>
#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionID.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/LambdaSkill.h>
#include <RobotAPI/libraries/skills/provider/SkillFactory.h>
#include <RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h>
 
namespace armarx::plugins
{
    void
    SkillProviderComponentPlugin::preOnInitComponent()
    {
    }
 
    void
    SkillProviderComponentPlugin::preOnConnectComponent()
    {
        auto& p = parent<SkillProviderComponentPluginUser>();
        p.getProxy(myPrx, -1);
    }
 
    void
    SkillProviderComponentPlugin::postOnConnectComponent()
    {
        auto& p = parent<SkillProviderComponentPluginUser>();
        const std::string providerName = p.getName();
 
        // register self to manager
        skills::ProviderInfo i{.providerId = skills::ProviderID{.providerName = providerName},
                               .providerInterface = myPrx,
                               .providedSkills = getSkillDescriptions()};
 
        ARMARX_INFO << "Adding provider to manager: " << i.providerId;
        manager->addProvider(i.toIce()); // add provider info to manager
    }
 
    void
    SkillProviderComponentPlugin::preOnDisconnectComponent()
    {
        auto& p = parent<SkillProviderComponentPluginUser>();
        std::string providerName = p.getName();
 
        auto id = skills::manager::dto::ProviderID{providerName};
        manager->removeProvider(id);
 
        // Drain all in-flight executions before tearing down the runtimes.
        // Joining must happen before destroying the SkillRuntime objects
        // because each runtime owns its own std::thread, which must not be
        // destroyed while joinable (would call std::terminate).
        std::vector<std::shared_ptr<skills::detail::SkillRuntime>> drained;
        {
            const std::unique_lock l(skillExecutionsMutex);
            for (auto& [id, runtime] : skillExecutions)
            {
                drained.push_back(std::move(runtime));
            }
            skillExecutions.clear();
        }
        for (auto& runtime : drained)
        {
            if (runtime && runtime->execution.joinable())
            {
                try
                {
                    runtime->execution.join();
                }
                catch (const std::exception& e)
                {
                    ARMARX_WARNING << "Failed to join skill execution thread during "
                                      "disconnect: "
                                   << e.what();
                }
            }
        }
        drained.clear();
 
        // remove all skills
        ARMARX_INFO << "Removing all skills";
        skillFactories.clear();
    }
 
    void
    SkillProviderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
    {
        std::string prefix = "skill.";
        properties->component(
            manager,
            "SkillMemory",
            prefix + "SkillManager",
            "The name of the SkillManager (or SkillMemory) proxy this provider belongs to.");
        properties->topic<armarx::skills::SkillEventListenerInterface>(
            "SkillEventListener", prefix + "tpc.sub.SkillEventListener");
    }
 
    void
    SkillProviderComponentPlugin::addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&& fac)
    {
        if (!fac)
        {
            return;
        }
 
        auto& p = parent<SkillProviderComponentPluginUser>();
        const std::string componentName = p.getName();
 
        const skills::ProviderID providerId({componentName});
 
        // lock skills map
        const std::unique_lock l(skillFactoriesMutex);
        auto skillId = fac->createSkillDescription(providerId).skillId;
 
        if (skillFactories.find(skillId) != skillFactories.end())
        {
            ARMARX_WARNING << "Try to add a skill factory for skill '" + skillId.toString() +
                                  "' which already exists in list. Ignoring this skill.";
            return;
        }
 
        ARMARX_INFO << "Adding skill `" << skillId << "` to component `" << componentName << "` .";
 
        skillFactories.emplace(skillId, std::move(fac));
 
 
        //        if (connected)
        //        {
        //            // if skill is added after onConnect we have to set the proxies manually.
        //            std::string providerName = parent().getName();
        //            s.first->second.skill->manager = manager;
        //            s.first->second.skill->providerName = providerName;
        //        }
    }
 
    skills::SkillBlueprint*
    SkillProviderComponentPlugin::addSkillFactory(const skills::SkillDescription& desc,
                                                  const skills::LambdaSkill::FunctionType& f)
    {
        return addSkillFactory<skills::LambdaSkill>(desc, f);
    }
 
    skills::SkillBlueprint*
    SkillProviderComponentPlugin::getSkillFactory(const armarx::skills::SkillID& skillId)
    {
        // NON BLOCKING: WE ASSERT THAT THE LOCK IS ALREADY TAKEN
        ARMARX_CHECK(skillId.isFullySpecified());
 
        if (skillFactories.count(skillId) == 0)
        {
            ARMARX_INFO << "Could not find a skill factory for id: " << skillId;
            return nullptr;
        }
 
        auto* facPtr = skillFactories.at(skillId).get();
        return static_cast<skills::SkillBlueprint*>(facPtr);
    }
 
    std::optional<skills::SkillStatusUpdate>
    SkillProviderComponentPlugin::getSkillExecutionStatus(
        const skills::SkillExecutionID& execId) const
    {
        ARMARX_CHECK(execId.skillId.isSkillSpecified());
 
        const std::shared_lock l(skillExecutionsMutex);
        auto it = skillExecutions.find(execId);
        if (it == skillExecutions.end())
        {
            ARMARX_WARNING << "Skill execution for skill '" + execId.skillId.toString() +
                                  "' not found!";
            return std::nullopt;
        }
 
        std::scoped_lock l2{it->second->skillStatusesMutex};
        return it->second->statusUpdate;
    }
 
    std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
    SkillProviderComponentPlugin::getSkillExecutionStatuses() const
    {
        std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillUpdates;
 
        const std::shared_lock l(skillExecutionsMutex);
        for (const auto& [key, impl] : skillExecutions)
        {
            const std::scoped_lock l2(impl->skillStatusesMutex);
            skillUpdates.insert({key, impl->statusUpdate});
        }
        return skillUpdates;
    }
 
    std::optional<skills::SkillDescription>
    SkillProviderComponentPlugin::getSkillDescription(const skills::SkillID& skillId) const
    {
        ARMARX_CHECK(skillId.isFullySpecified());
 
        const std::shared_lock l(skillFactoriesMutex);
        if (skillFactories.find(skillId) == skillFactories.end())
        {
            std::stringstream ss;
            ss << "Skill description for skill '" + skillId.toString() +
                      "' not found! Found instead: {"
               << "\n";
            for (const auto& [k, _] : skillFactories)
            {
                ss << "\t" << k.toString() << "\n";
            }
            ss << "}";
            ARMARX_WARNING << ss.str();
 
            return std::nullopt;
        }
 
        return skillFactories.at(skillId)->createSkillDescription(*skillId.providerId);
    }
 
    std::map<skills::SkillID, skills::SkillDescription>
    SkillProviderComponentPlugin::getSkillDescriptions() const
    {
        std::map<skills::SkillID, skills::SkillDescription> skillDesciptions;
        const std::shared_lock l(skillFactoriesMutex);
        for (const auto& [key, fac] : skillFactories)
        {
            ARMARX_CHECK(key.isFullySpecified());
            skillDesciptions.insert({key, fac->createSkillDescription(*key.providerId)});
        }
        return skillDesciptions;
    }
 
    skills::SkillStatusUpdate
    SkillProviderComponentPlugin::executeSkill(
        const skills::SkillExecutionRequest& executionRequest)
    {
        ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
 
        skills::SkillExecutionID executionId{.skillId = executionRequest.skillId,
                                             .executorName = executionRequest.executorName,
                                             .executionStartedTime =
                                                 armarx::core::time::DateTime::Now()};
 
        skills::SkillStatusUpdate ret{
            {executionId, executionRequest.parameters, executionRequest.callbackInterface}};
 
        std::shared_ptr<skills::detail::SkillRuntime> runtime;
        std::vector<std::shared_ptr<skills::detail::SkillRuntime>> finishedToJoin;
        {
            auto l1 = std::unique_lock{skillFactoriesMutex};
 
            const auto& fac = getSkillFactory(executionId.skillId);
            ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
 
            {
                const std::unique_lock l2{skillExecutionsMutex};
 
                // Drop terminated executions from the map. Joining must be
                // done outside the lock to avoid blocking other API calls
                // while we wait for a thread to finish.
                finishedToJoin = collectFinishedExecutions_locked();
 
                runtime = std::make_shared<skills::detail::SkillRuntime>(
                    fac,
                    executionId,
                    executionRequest.parameters,
                    executionRequest.callbackInterface);
                skillExecutions.emplace(executionId, runtime);
                auto const loggingLevel = parent().getEffectiveLoggingLevel();
                ARMARX_VERBOSE << "Setting skill runtime's logging level to `"
                               << armarx::LogSender::levelToString(loggingLevel) << "`.";
                runtime->setLocalMinimumLoggingLevel(loggingLevel);
 
                // Capture the shared_ptr by VALUE so the runtime is kept
                // alive for as long as the lambda runs, even if the map
                // entry is erased concurrently. `ret` is captured by
                // reference because we join the thread before returning
                // from this function.
                runtime->execution = std::thread(
                    [runtime, &ret]()
                    {
                        try
                        {
                            auto x = runtime->executeSkill();
                            ret.result = x.result;
                            ret.status = armarx::skills::toSkillStatus(x.status);
                        }
                        catch (std::exception& e)
                        {
                            ARMARX_WARNING << "Got an uncaught exception when executing a "
                                              "skill. Exception was: "
                                           << e.what();
                        }
                    });
            }
        } // release lock. We don't know how long the skill needs to finish and we have to release the lock for being able to abort the execution
 
        // Drain previously finished executions outside the lock.
        for (auto& finished : finishedToJoin)
        {
            if (finished && finished->execution.joinable())
            {
                finished->execution.join();
            }
        }
        finishedToJoin.clear();
 
        if (runtime && runtime->execution.joinable())
        {
            runtime->execution.join();
        }
        return ret;
    }
 
    skills::SkillExecutionID
    SkillProviderComponentPlugin::executeSkillAsync(
        const skills::SkillExecutionRequest& executionRequest)
    {
        ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
 
        skills::SkillExecutionID executionId;
 
        std::shared_ptr<skills::detail::SkillRuntime> runtime;
        std::vector<std::shared_ptr<skills::detail::SkillRuntime>> finishedToJoin;
        {
            auto l1 = std::unique_lock{skillFactoriesMutex};
 
            const auto& fac = getSkillFactory(executionRequest.skillId);
            ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionRequest.skillId;
 
            {
                const std::unique_lock l2{skillExecutionsMutex};
 
                // Drop terminated executions from the map.
                finishedToJoin = collectFinishedExecutions_locked();
 
                executionId = skills::SkillExecutionID{executionRequest.skillId,
                                                       executionRequest.executorName,
                                                       armarx::core::time::DateTime::Now()};
 
                if (skillExecutions.count(executionId) > 0)
                {
                    ARMARX_ERROR << "SkillsExecutionID already exists! This is undefined behaviour "
                                    "and should not occur!";
                }
 
                runtime = std::make_shared<skills::detail::SkillRuntime>(
                    fac,
                    executionId,
                    executionRequest.parameters,
                    executionRequest.callbackInterface);
                skillExecutions.emplace(executionId, runtime);
                auto const loggingLevel = parent().getEffectiveLoggingLevel();
                ARMARX_INFO << "Setting skill runtime's logging level to `"
                            << armarx::LogSender::levelToString(loggingLevel) << "`.";
                runtime->setLocalMinimumLoggingLevel(loggingLevel);
 
                // Capture the shared_ptr by VALUE. The function returns
                // before the thread finishes, so a reference capture would
                // dangle as soon as the local `runtime` variable is
                // destroyed -> use-after-free.
                runtime->execution = std::thread(
                    [runtime]()
                    {
                        try
                        {
                            auto x = runtime->executeSkill();
                            (void)x;
                        }
                        catch (std::exception& e)
                        {
                            ARMARX_WARNING << "Got an uncaught exception when executing a "
                                              "skill. Exception was: "
                                           << e.what();
                        }
                    });
            }
        }
 
        // Drain previously finished executions outside the lock.
        for (auto& finished : finishedToJoin)
        {
            if (finished && finished->execution.joinable())
            {
                finished->execution.join();
            }
        }
        finishedToJoin.clear();
 
        // wait until skill is constructed. This assures, that a status update exists.
        while (true)
        {
            {
                std::scoped_lock l(runtime->skillStatusesMutex);
 
                if (runtime->statusUpdate.hasBeenConstructed())
                {
                    break;
                }
            }
 
            std::this_thread::sleep_for(std::chrono::milliseconds(20));
        }
 
        return executionId;
    }
 
    bool
    SkillProviderComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
                                                        const armarx::aron::data::DictPtr& input)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified());
 
        std::shared_ptr<skills::detail::SkillRuntime> runtime;
        {
            std::shared_lock l{skillExecutionsMutex};
            auto it = skillExecutions.find(executionId);
            if (it == skillExecutions.end())
            {
                ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
                                   "' found! Ignoring prepareSkill request.";
                return false;
            }
            runtime = it->second;
        }
 
        std::scoped_lock l2{runtime->skillStatusesMutex};
        if (runtime->statusUpdate.status != skills::SkillStatus::Preparing)
        {
            ARMARX_INFO << "Could not prepare the skill '" + executionId.skillId.toString() +
                               "' because its not in preparing phase.";
            return false;
        }
 
        runtime->updateSkillParameters(input);
        return true;
    }
 
    bool
    SkillProviderComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified());
 
        std::shared_ptr<skills::detail::SkillRuntime> runtime;
        {
            std::shared_lock l(skillExecutionsMutex);
            auto it = skillExecutions.find(executionId);
            if (it == skillExecutions.end())
            {
                ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
                                   "' found! Ignoring abortSkill request.";
                return false;
            }
            runtime = it->second;
        }
 
        runtime->stopSkill();
 
        while (true)
        {
            {
                std::scoped_lock l2(runtime->skillStatusesMutex);
                auto status = runtime->statusUpdate;
 
                if (status.hasBeenTerminated())
                {
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(20));
        }
 
        return true;
    }
 
    bool
    SkillProviderComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified());
 
        std::shared_ptr<skills::detail::SkillRuntime> runtime;
        {
            std::shared_lock l(skillExecutionsMutex);
            auto it = skillExecutions.find(executionId);
            if (it == skillExecutions.end())
            {
                ARMARX_INFO << "No active execution for skill '" + executionId.skillId.toString() +
                                   "' found! Ignoring abortSkill request.";
                return false;
            }
            runtime = it->second;
        }
 
        runtime->stopSkill();
        return true;
    }
 
    void
    SkillProviderComponentPlugin::updateSubSkillStatus(
        const skills::SkillStatusUpdate& statusUpdate)
    {
        // Snapshot the runtimes under the lock so we can call into them
        // without holding the lock (sub-skill status updates can be slow).
        std::vector<std::pair<skills::SkillExecutionID,
                              std::shared_ptr<skills::detail::SkillRuntime>>>
            snapshot;
        {
            std::shared_lock l(skillExecutionsMutex);
            snapshot.reserve(skillExecutions.size());
            for (const auto& [id, runtime] : skillExecutions)
            {
                snapshot.emplace_back(id, runtime);
            }
        }
        for (auto& [id, runtime] : snapshot)
        {
            ARMARX_DEBUG << "updating subskill status for " << id.toString() << " with "
                         << statusUpdate.executionId.toString();
            runtime->updateSubSkillStatus(statusUpdate);
        }
    }
 
    const skills::manager::dti::SkillManagerInterfacePrx&
    SkillProviderComponentPlugin::skillManager() const
    {
        return manager;
    }
 
    std::vector<std::shared_ptr<skills::detail::SkillRuntime>>
    SkillProviderComponentPlugin::collectFinishedExecutions_locked()
    {
        // Caller MUST hold an exclusive lock on skillExecutionsMutex.
        // We move terminated entries OUT of the map (under the lock) and
        // return them, so the caller can join the threads outside the lock.
        // The caller is responsible for joining; the SkillRuntime will be
        // destroyed when its shared_ptr ref count drops to zero, which is
        // safe only AFTER the embedded std::thread has been joined.
        std::vector<std::shared_ptr<skills::detail::SkillRuntime>> ret;
        for (auto it = skillExecutions.begin(); it != skillExecutions.end();)
        {
            const auto& runtime = it->second;
            bool terminated = false;
            {
                std::scoped_lock statusLock(runtime->skillStatusesMutex);
                terminated = runtime->statusUpdate.hasBeenTerminated();
            }
            if (terminated)
            {
                ret.push_back(std::move(it->second));
                it = skillExecutions.erase(it);
            }
            else
            {
                ++it;
            }
        }
        return ret;
    }
} // namespace armarx::plugins
 
namespace armarx
{
    SkillProviderComponentPluginUser::SkillProviderComponentPluginUser()
    {
        addPlugin(plugin);
    }
 
    IceUtil::Optional<skills::provider::dto::SkillDescription>
    SkillProviderComponentPluginUser::getSkillDescription(
        const skills::provider::dto::SkillID& skillId,
        const Ice::Current& /*unused*/)
    {
        auto id = skills::SkillID::FromIce(skillId, skills::ProviderID{.providerName = getName()});
        auto o = plugin->getSkillDescription(id);
        if (o.has_value())
        {
            return o->toProviderIce();
        }
        return {};
    }
 
    skills::provider::dto::SkillDescriptionMap
    SkillProviderComponentPluginUser::getSkillDescriptions(const Ice::Current& /*unused*/)
    {
        skills::provider::dto::SkillDescriptionMap ret;
        for (const auto& [k, v] : plugin->getSkillDescriptions())
        {
            ret.insert({k.toProviderIce(), v.toProviderIce()});
        }
        return ret;
    }
 
    IceUtil::Optional<skills::provider::dto::SkillStatusUpdate>
    SkillProviderComponentPluginUser::getSkillExecutionStatus(
        const skills::provider::dto::SkillExecutionID& executionId,
        const Ice::Current& /*unused*/)
    {
        auto execId = skills::SkillExecutionID::FromIce(
            executionId, skills::ProviderID{.providerName = getName()});
        auto o = plugin->getSkillExecutionStatus(execId);
        if (o.has_value())
        {
            return o->toProviderIce();
        }
        return {};
    }
 
    skills::provider::dto::SkillStatusUpdateMap
    SkillProviderComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& /*unused*/)
    {
        skills::provider::dto::SkillStatusUpdateMap ret;
        for (const auto& [k, v] : plugin->getSkillExecutionStatuses())
        {
            ret.insert({k.toProviderIce(), v.toProviderIce()});
        }
        return ret;
    }
 
    // Please not that this method waits until the skill can be scheduled!
    skills::provider::dto::SkillStatusUpdate
    SkillProviderComponentPluginUser::executeSkill(
        const skills::provider::dto::SkillExecutionRequest& info,
        const Ice::Current& /*unused*/)
    {
        auto exec = skills::SkillExecutionRequest::FromIce(
            info, skills::ProviderID{.providerName = getName()});
        auto up = this->plugin->executeSkill(exec);
        return up.toProviderIce();
    }
 
    skills::provider::dto::SkillExecutionID
    SkillProviderComponentPluginUser::executeSkillAsync(
        const skills::provider::dto::SkillExecutionRequest& info,
        const Ice::Current& current /*unused*/)
    {
        auto exec = skills::SkillExecutionRequest::FromIce(
            info, skills::ProviderID{.providerName = getName()});
        auto id = this->plugin->executeSkillAsync(exec);
        return id.toProviderIce();
    }
 
    skills::provider::dto::ParameterUpdateResult
    SkillProviderComponentPluginUser::updateSkillParameters(
        const skills::provider::dto::SkillExecutionID& id,
        const aron::data::dto::DictPtr& input,
        const Ice::Current& current /*unused*/)
    {
        skills::provider::dto::ParameterUpdateResult res;
 
        auto exec =
            skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
        auto prep = armarx::aron::data::Dict::FromAronDictDTO(input);
        res.success = this->plugin->updateSkillParameters(exec, prep);
        return res;
    }
 
    skills::provider::dto::AbortSkillResult
    SkillProviderComponentPluginUser::abortSkill(const skills::provider::dto::SkillExecutionID& id,
                                                 const Ice::Current& /*unused*/)
    {
        skills::provider::dto::AbortSkillResult res;
        auto exec =
            skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
        res.success = this->plugin->abortSkill(exec);
        return res;
    }
 
    skills::provider::dto::AbortSkillResult
    SkillProviderComponentPluginUser::abortSkillAsync(
        const skills::provider::dto::SkillExecutionID& id,
        const Ice::Current& /*unused*/)
    {
        skills::provider::dto::AbortSkillResult res;
        auto exec =
            skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
        res.success = this->plugin->abortSkillAsync(exec);
        return res;
    }
 
    void
    SkillProviderComponentPluginUser::reportSkillEvent(
        const skills::provider::dto::SkillStatusUpdate& statusUpdate,
        const std::string& providerName,
        const Ice::Current& /*current*/)
    {
        auto status = skills::SkillStatusUpdate::FromIce(statusUpdate);
        status.executionId.skillId.providerId.emplace().providerName = providerName;
        plugin->updateSubSkillStatus(status);
    }
 
    const std::experimental::observer_ptr<plugins::SkillProviderComponentPlugin>&
    SkillProviderComponentPluginUser::getSkillProviderPlugin() const
    {
        return plugin;
    }
} // namespace armarx