SkillManagerWrapper.cpp

Go to the documentation of this file.

#include "SkillManagerWrapper.h"
 
#include <mutex>
#include <optional>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/libraries/skills/core/SkillExecutionRequest.h"
 
namespace armarx::skills::gui
{
    using StatusMap = std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>;
    using SkillMap =
        std::map<skills::ProviderID, std::map<skills::SkillID, skills::SkillDescription>>;
 
    StatusMap
    SkillManagerWrapper::fetchExecutionsFromMemory()
    {
        if (!memory)
        {
            // check if null
            return {};
        }
 
        try
        {
            std::scoped_lock l(mutex_memory);
 
            // we return this map
            StatusMap statusMap;
 
            auto currentManagerStatuses =
                memory->ice_invocationTimeout(10000)->getSkillExecutionStatuses();
 
            // iterate over raw data and convert to common types
            for (const auto& [k, v] : currentManagerStatuses)
            {
                auto executionId = skills::SkillExecutionID::FromIce(k);
                auto statusUpdate = skills::SkillStatusUpdate::FromIce(v);
 
                // update maps
                statusMap[executionId] = statusUpdate;
            }
            return statusMap;
        }
        catch (Ice::Exception const& e)
        {
            ARMARX_WARNING
                << "Unhandled Ice exception encountered while updating executions. Exception was: "
                << e;
            emit connectionUpdate("Could not fetch executions", e.what());
            emit disableAutoUpdate();
            return {};
        }
        catch (...)
        {
            ARMARX_WARNING << "Unknown exception encountered while updating executions.";
            return {};
        }
    }
 
    // check if search strings occur in the skill name in order
    bool
    matches(std::string skillName, std::vector<std::string>& searches)
    {
        size_t index = 0;
        for (std::string& substring : searches)
        {
            size_t occurance = skillName.find(substring, index);
            if (occurance == std::string::npos)
            {
                return false;
            }
 
 
            // we found an occurance
            index = occurance;
        }
        return true;
    }
 
    SkillManagerWrapper::Snapshot
    SkillManagerWrapper::filterUpdate(Snapshot update)
    {
        // empty search => do not filter
        if (this->currentSkillSearch.empty())
        {
            return update;
        }
 
        Snapshot filtered;
 
        // for now we don't change the executions
        filtered.statuses = update.statuses;
 
        std::vector<std::string> substrings;
 
        {
            std::vector<std::string> rawSubstrings = simox::alg::split(currentSkillSearch);
            for (auto& string : rawSubstrings)
            {
                substrings.push_back(simox::alg::to_lower(string));
            }
        }
 
 
        for (auto& provider_and_descrMap : update.skills)
        {
            using DescriptionMap = std::map<skills::SkillID, skills::SkillDescription>;
 
            DescriptionMap& descriptionMap = provider_and_descrMap.second;
            skills::ProviderID provider = provider_and_descrMap.first;
 
            for (auto& skill_and_description : descriptionMap)
            {
                skills::SkillID sid = skill_and_description.first;
                skills::SkillDescription descr = skill_and_description.second;
 
                if (matches(simox::alg::to_lower(sid.skillName), substrings))
                {
                    // add to map
                    filtered.skills[provider][sid] = descr;
                }
            }
        }
 
        return filtered;
    }
 
    SkillMap
    SkillManagerWrapper::fetchSkillsFromMemory()
    {
        if (!memory)
        {
            return {};
        }
 
        try
        {
            std::scoped_lock l(mutex_memory);
 
            SkillMap skills;
 
            auto managerSkills = memory->ice_invocationTimeout(5000)->getSkillDescriptions();
 
            for (const auto& [sid, desc] : managerSkills)
            {
                auto description = skills::SkillDescription::FromIce(desc);
                auto skillId = skills::SkillID::FromIce(sid);
                auto providerId = skillId.providerId.value_or(
                    skills::ProviderID{.providerName = "UNKNOWN PROVIDER NAME"});
 
                ARMARX_CHECK(skillId.isFullySpecified());
 
                auto& providedSkillsMap = skills[providerId]; // create new if not existing
                providedSkillsMap.insert({skillId, description});
            }
 
            return skills;
        }
        catch (Ice::Exception const& e)
        {
            ARMARX_WARNING
                << "Unhandled Ice exception encountered while updating skills. Exception was: "
                << e;
            emit connectionUpdate("Could not fetch skills", e.what());
            emit disableAutoUpdate();
            return {};
        }
        catch (...)
        {
            ARMARX_WARNING << "Unknown exception encountered while updating skills.";
            return {};
        }
    }
 
    void
    SkillManagerWrapper::connectMemory(
        skills::manager::dti::SkillManagerInterfacePrx const& updatedMemory)
    {
        std::scoped_lock l(mutex_memory);
        this->memory = updatedMemory;
    }
 
    void
    SkillManagerWrapper::disconnectMemory()
    {
        // clear memory
 
        std::scoped_lock l(mutex_memory);
        this->memory = nullptr;
    }
 
    void
    SkillManagerWrapper::acceptSearchRequest(std::string const& search)
    {
        this->currentSkillSearch = search;
        emit searchAccepted();
    }
 
    void
    SkillManagerWrapper::stopAllExecutions()
    {
        ARMARX_IMPORTANT << "Stopping all running executions.";
 
        StatusMap executions;
 
        // we ALWAYS want the newest information when stopping all!
        // e.g. there is some new skill not known to the GUI which we explicitely want to stop too.
        // the stop-all function is often used in an emergency, so we'll live with the extra call...
        try
        {
            executions = this->fetchExecutionsFromMemory();
        }
        catch (...) // if any error occurs, we use the snapshot as backup. better to miss a skill
            // than to not do anything.
        {
            executions = this->getExecutions();
        }
 
        for (auto& [executionId, status] : executions)
        {
            // select all running executions...
            if (!status.hasBeenTerminated())
            {
                // ... and kill them.
                this->stopExecution(executionId, 3);
            }
        }
    }
 
    void
    SkillManagerWrapper::updateFromMemory()
    {
        std::scoped_lock l(mutex_snapshot);
 
        snapshot.skills = fetchSkillsFromMemory();
        snapshot.statuses = fetchExecutionsFromMemory();
 
        // notify registered widgets of update
        emit updateAvailable(filterUpdate(snapshot));
    }
 
    std::vector<Parameters>
    SkillManagerWrapper::getParameterHistoryForSkill(const skills::SkillID& sid)
    {
        if (this->skillParameterExecutionHistory.find(sid) ==
            this->skillParameterExecutionHistory.end())
        {
            return {};
        }
        return this->skillParameterExecutionHistory[sid];
    }
 
    void
    SkillManagerWrapper::addParametersToHistory(const skills::SkillID& sid,
                                                const Parameters& params)
    {
        if (this->skillParameterExecutionHistory.find(sid) ==
            this->skillParameterExecutionHistory.end())
        {
            // no history exists for this skill. Assign new
            this->skillParameterExecutionHistory.insert({sid, {params}});
        }
        else
        {
            // history already exists. Append to list
            this->skillParameterExecutionHistory[sid].emplace_back(params);
        }
    }
 
    std::optional<Parameters>
    SkillManagerWrapper::getLatestParametersForSkill(const skills::SkillID& sid)
    {
        auto parameterList = getParameterHistoryForSkill(sid);
        if (parameterList.empty())
        {
            return std::nullopt;
        }
        return parameterList.back();
    }
 
    const std::optional<ProviderID>
    SkillManagerWrapper::findFirstProvider(SkillMap const& map, SkillID const& skillId)
    {
        // check if id already contains a provider. If so, this function should not have been called!
        if (skillId.isProviderSpecified())
        {
            ARMARX_WARNING << "The memory snapshot was searched for any provider, when a provider "
                              "was specified.";
            // we continue, but this might result in unexpected behaviour...
        }
 
        for (auto& [prov, skillMap] : map)
        {
            for (auto& [skill, desc] : skillMap)
            {
                if (skill == skillId)
                {
                    return prov;
                }
            }
        }
        return std::nullopt;
    }
 
    SkillMap
    SkillManagerWrapper::getSkillsFiltered()
    {
        std::scoped_lock l(mutex_snapshot);
 
        Snapshot filtered = filterUpdate(snapshot);
 
        return filtered.skills;
    }
 
    SkillMap
    SkillManagerWrapper::getSkills()
    {
        std::scoped_lock l(mutex_snapshot);
 
        return snapshot.skills;
    }
 
    StatusMap
    SkillManagerWrapper::getExecutions()
    {
        std::scoped_lock l(mutex_snapshot);
        return snapshot.statuses;
    }
 
    void
    SkillManagerWrapper::stopExecution(skills::SkillExecutionID const& executionId,
                                       const unsigned int max_retries)
    {
        // memory???
        if (!memory)
        {
            return;
        }
 
        unsigned int retries = max_retries;
        bool retry = false;
 
        do
        {
            try
            {
                ARMARX_INFO << "Aborting skill '" << executionId.skillId.skillName << "'...";
                std::scoped_lock l(mutex_memory);
                this->memory->ice_invocationTimeout(5000)->abortSkillAsync(
                    executionId.toManagerIce());
            }
            catch (Ice::Exception const& e)
            {
                retry = true;
                ARMARX_ERROR << "Unhandeled Ice exception while aborting skill '"
                             << executionId.skillId.skillName << "'.";
                emit connectionUpdate("Could not abort skill " + executionId.skillId.skillName,
                                      e.what());
            }
            catch (...)
            {
                retry = true;
                ARMARX_ERROR << "Unhandled error while aborting skill '"
                             << executionId.skillId.skillName << "'.";
            }
 
            if (retry)
            {
                retries -= 1;
 
                if (retries > 0)
                {
                    ARMARX_WARNING << "There where errors aborting skills.  Retrying...";
                }
                else
                {
                    ARMARX_ERROR << "Couldn't abort all skills after " << max_retries
                                 << " tries.  Giving up.";
                    retry = false;
                }
            }
        } while (retry);
    }
 
    void
    SkillManagerWrapper::startExecutionWithParams(skills::SkillID& skillId,
                                                  aron::data::DictPtr const params)
    {
        // Memory???
        if (!memory)
        {
            return;
        }
 
        auto providerId = skillId.providerId;
        if (!providerId.has_value())
        {
            ARMARX_IMPORTANT << "The skill: '" << skillId.skillName
                             << "' has been requested to be executed, but no provider was "
                                "given. Aborting...";
            return;
        }
 
        std::map<skills::SkillID, skills::SkillDescription> skillDescriptions;
        if (this->UPDATE_ON_EXECUTION_REQUEST)
        {
            skillDescriptions = this->fetchSkillsFromMemory().at(providerId.value());
        }
        else
        {
            skillDescriptions = this->getSkills().at(providerId.value());
        }
 
        if (skillDescriptions.find(skillId) == skillDescriptions.end())
        {
            ARMARX_IMPORTANT << "The Skill: '" << skillId.skillName
                             << "' has been requested to be executed, but no skill description was "
                                "found. Aborting...";
            return;
        }
 
        char hostname[HOST_NAME_MAX];
        gethostname(hostname, HOST_NAME_MAX);
 
        skills::SkillExecutionRequest req{
            .skillId = skillId,
            .executorName = "Skills.Manager GUI (hostname: " + std::string(hostname) + ")",
            .parameters = params};
 
        ARMARX_CHECK(skillId.isFullySpecified()); // sanity check
 
        ARMARX_IMPORTANT << "Executing skill from GUI: " << skillId << ".";
 
        try
        {
            std::scoped_lock l(mutex_memory);
            memory->ice_invocationTimeout(5000)->executeSkillAsync(req.toManagerIce());
        }
        catch (Ice::Exception const& e)
        {
            ARMARX_ERROR << "Unhandeled Ice exception while executing skill '" << skillId.skillName
                         << "'. Aborting...";
            emit connectionUpdate("Execution failed of skill " + skillId.skillName, e.what());
        }
        catch (...)
        {
            ARMARX_ERROR << "Unhandled error while executing skill '" << skillId.skillName
                         << "'. Aborting...";
        }
    }
 
 
} // namespace armarx::skills::gui