SkillProxy.cpp

Go to the documentation of this file.

#include "SkillProxy.h"
 
#include <chrono>
#include <mutex>
#include <optional>
#include <sstream>
#include <string>
#include <thread>
#include <IceUtil/Optional.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "RobotAPI/libraries/aron/core/data/variant/container/Dict.h"
#include "RobotAPI/libraries/skills/core/SkillDescription.h"
#include "RobotAPI/libraries/skills/core/SkillExecutionID.h"
#include "RobotAPI/libraries/skills/core/SkillID.h"
#include "RobotAPI/libraries/skills/core/SkillStatusUpdate.h"
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
 
namespace armarx
{
    namespace skills
    {
        SkillUpdateManager GlobalSkillUpdateManager;
 
        SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
                               const SkillID& skillId) :
            manager(manager)
        {
            ARMARX_CHECK_NOT_NULL(manager);
            IceUtil::Optional<manager::dto::SkillDescription> description =
                manager->getSkillDescription(skillId.toManagerIce());
            if (description)
            {
                skillDescription = SkillDescription::FromIce(description.value());
                ARMARX_CHECK(skillDescription.skillId.isFullySpecified());
            }
            else
            {
                std::stringstream reason;
                reason << "No skill with ID " << skillId << " found";
                throw error::SkillNotFoundException(__PRETTY_FUNCTION__, reason.str());
            }
 
            callbackId = GlobalSkillUpdateManager.addCallback(
                [this](const skills::SkillStatusUpdate& statusUpdate)
                { this->updateSubSkillStatus(statusUpdate); });
        }
 
        SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
                               const SkillDescription& skillDesc) :
            manager(manager), skillDescription(skillDesc)
        {
            ARMARX_CHECK_NOT_NULL(manager);
            ARMARX_CHECK(skillDesc.skillId.isFullySpecified());
 
            callbackId = GlobalSkillUpdateManager.addCallback(
                [this](const skills::SkillStatusUpdate& statusUpdate)
                { this->updateSubSkillStatus(statusUpdate); });
        }
 
 
        SkillProxy::~SkillProxy()
        {
            GlobalSkillUpdateManager.removeCallback(callbackId);
        }
 
        SkillDescription
        SkillProxy::getSkillDescription() const
        {
            return skillDescription;
        }
 
        SkillID
        SkillProxy::getSkillId() const
        {
            return skillDescription.skillId;
        }
 
        TerminatedSkillStatusUpdate
        SkillProxy::executeSkill(const std::string& executorName,
                                 const aron::data::DictPtr& params) const
        {
            ARMARX_CHECK_NOT_NULL(manager);
            skills::manager::dto::SkillExecutionRequest req;
            req.executorName = executorName;
            req.parameters = aron::data::Dict::ToAronDictDTO(params);
            req.skillId = skillDescription.skillId.toManagerIce();
 
            auto terminatingUpdateIce = manager->executeSkill(req);
            return TerminatedSkillStatusUpdate::FromIce(terminatingUpdateIce);
        }
 
        SkillExecutionID
        SkillProxy::executeSkillAsync(const std::string& executorName,
                                      const aron::data::DictPtr& params) const
        {
            ARMARX_CHECK_NOT_NULL(manager);
            skills::manager::dto::SkillExecutionRequest req;
            req.executorName = executorName;
            req.parameters = aron::data::Dict::ToAronDictDTO(params);
            req.skillId = skillDescription.skillId.toManagerIce();
 
            auto execIdIce = manager->executeSkillAsync(req);
            return SkillExecutionID::FromIce(execIdIce);
        }
 
        std::optional<TerminatedSkillStatusUpdate>
        SkillProxy::join(const SkillExecutionID& executionId,
                         const armarx::Duration max_wait_time,
                         const armarx::Frequency check_frequency) const
        {
            ARMARX_CHECK_NOT_NULL(manager);
 
            std::optional<TerminatedSkillStatusUpdate> ret = std::nullopt;
 
            int const max_tries =
                check_frequency.toHertzDouble() * max_wait_time.toSecondsDouble() + 1e-5;
            ARMARX_VERBOSE << VAROUT(max_tries);
            int tries = 0;
 
            armarx::Metronome m{check_frequency};
 
            ARMARX_VERBOSE << "Waiting for skill " << QUOTED(executionId.toString())
                           << " to finish...";
 
            while (true)
            {
                SkillStatusUpdate statusUpdate;
 
                {
                    std::scoped_lock l{lastUpdatesMutex};
                    if (lastUpdates.count(executionId.toString()) > 0)
                    {
                        ARMARX_DEBUG << "Status update received.";
                        statusUpdate = lastUpdates.at(executionId.toString());
                    }
                    else
                    {
                        tries += 1;
                        if (tries > max_tries)
                        {
                            ARMARX_WARNING << "Skill " << QUOTED(executionId.toString())
                                           << " does not seem to have been started "
                                              "successfully after "
                                           << max_wait_time
                                           << ".  "
                                              "Giving up.";
                            break;
                        }
                    }
                }
 
                if (statusUpdate.hasBeenTerminated())
                {
                    // TODO do conversion without ice
                    ret = TerminatedSkillStatusUpdate::FromIce(statusUpdate.toProviderIce());
                    ARMARX_VERBOSE << "Got skill termination status for "
                                   << statusUpdate.executionId.toString();
                    break;
                }
 
                m.waitForNextTick();
            }
 
            return ret;
        }
 
        void
        SkillProxy::updateSubSkillStatus(const skills::SkillStatusUpdate& statusUpdate)
        {
            ARMARX_DEBUG << VAROUT(skillDescription.skillId.skillName);
            ARMARX_DEBUG << VAROUT(statusUpdate.executionId.skillId.skillName);
 
            if (skillDescription.skillId.skillName == statusUpdate.executionId.skillId.skillName)
            {
                // TODO only update if new status
                // if (lastUpdates.count(update.executionId) == 0 or
                //     lastUpdates.at(update.executionId) < update)
                // {
                //     lastUpdates[update.executionId] = update;
                // }
                // else
                // {
                //     ARMARX_INFO << "Ignoring.";
                // }
                {
                    std::scoped_lock l{lastUpdatesMutex};
                    ARMARX_DEBUG << "Updating status for skill "
                                 << QUOTED(statusUpdate.executionId.toString());
                    lastUpdates[statusUpdate.executionId.toString()] = statusUpdate;
                }
            }
            else
            {
                ARMARX_DEBUG << "Ignoring status update for skill "
                             << QUOTED(statusUpdate.executionId.skillId.skillName);
            }
        }
 
        bool
        SkillProxy::abortSkill(const SkillExecutionID& id) const
        {
            ARMARX_CHECK_NOT_NULL(manager);
            auto r = manager->abortSkill(id.toManagerIce());
            return r.success;
        }
 
        bool
        SkillProxy::abortSkillAsync(const SkillExecutionID& id) const
        {
            ARMARX_CHECK_NOT_NULL(manager);
            auto r = manager->abortSkillAsync(id.toManagerIce());
            return r.success;
        }
 
        aron::data::DictPtr
        SkillProxy::getRootProfileParameters() const
        {
            return skillDescription.rootProfileDefaults;
        }
 
        std::optional<SkillStatusUpdate>
        SkillProxy::getExecutionStatus(const SkillExecutionID& executionId) const
        {
            IceUtil::Optional<::armarx::skills::manager::dto::SkillStatusUpdate> statusUpdate =
                manager->getSkillExecutionStatus(executionId.toManagerIce());
 
            if (not statusUpdate)
            {
                return std::nullopt;
            }
 
            auto status = skills::SkillStatusUpdate::FromIce(statusUpdate.value());
            // status.executionId.skillId.providerId.emplace().providerName = providerName;
            return status;
        }
    } // namespace skills
} // namespace armarx