SkillExecutionHandle.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/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SkillExecutionHandle.h"
 
#include <memory>
#include <optional>
#include <utility>
#include <vector>
 
#include <range/v3/algorithm/all_of.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/indirect.hpp>
#include <range/v3/view/map.hpp>
#include <range/v3/view/transform.hpp>
#include <range/v3/view/zip.hpp>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "RobotAPI/libraries/skills/core/SkillID.h"
#include <RobotAPI/libraries/skills/core/SkillExecutionID.h>
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
namespace armarx::skills
{
 
 
    SkillExecutionHandle::~SkillExecutionHandle()
    {
        ARMARX_VERBOSE << "Destroying SkillExecutionHandle for subskill "
                       << proxy_->getSkillId().toString();
 
        if (not isJoined_)
        {
            // The following commented out code uses getExecutionStatus() which is very inefficient.
            // Hence, we don't use this at the moment.
            // if (not getExecutionStatus()->hasBeenTerminated())
            {
                ARMARX_INFO << "Joining skill " << QUOTED(proxy_->getSkillId().toString())
                            << " on handle destruction.";
                const auto status = join();
                ARMARX_INFO << "Joined skill " << QUOTED(proxy_->getSkillId().toString()) << ".";
 
                if (skillExecutionFailed(status))
                {
                    ARMARX_ERROR << "Skill execution " << QUOTED(proxy_->getSkillId().toString())
                                 << " failed";
                }
            }
        }
    }
 
    SkillExecutionHandle::SkillExecutionHandle(SkillProxyPtr proxy,
                                               skills::SkillExecutionID executionId) :
        proxy_(std::move(proxy)), executionId_(std::move(executionId))
    {
        ARMARX_CHECK_NOT_NULL(proxy_);
    }
 
    std::optional<TerminatedSkillStatusUpdate>
    SkillExecutionHandle::join()
    {
        ARMARX_INFO << "Joining subskill " << QUOTED(proxy_->getSkillId().toString());
 
        std::optional<TerminatedSkillStatusUpdate> result = proxy_->join(executionId_);
        isJoined_ = true;
 
        return result;
    }
 
    bool
    SkillExecutionHandle::abortAsync()
    {
        ARMARX_INFO << "Aborting subskill " << QUOTED(proxy_->getSkillId().toString());
 
        return proxy_->abortSkillAsync(executionId_);
    }
 
    std::optional<SkillStatusUpdate>
    SkillExecutionHandle::getExecutionStatus()
    {
        // NOTE: proxy_->getExecutionStatus is very inefficient and can take up to 500ms.
        //   Hence, it should not be used at the moment.
 
        return proxy_->getExecutionStatus(executionId_);
    }
 
    namespace util
    {
 
        JoinAllResult::
        operator bool() const
        {
            return ranges::all_of(
                ranges::views::values(updates),
                [](const std::optional<armarx::skills::TerminatedSkillStatusUpdate>& update)
                { return skills::skillExecutionSucceeded(update); });
        }
 
        std::vector<armarx::skills::SkillExecutionID>
        JoinAllResult::failedSkillExecutionIDs() const
        {
            const auto failed = [this](const armarx::skills::SkillExecutionID& executionId)
            { return armarx::skills::skillExecutionFailed(updates.at(executionId)); };
 
            return ranges::views::keys(updates) | ranges::views::filter(failed) | ranges::to_vector;
        }
 
        std::vector<armarx::skills::SkillID>
        JoinAllResult::failedSkillIDs() const
        {
            const auto failed = [this](const armarx::skills::SkillExecutionID& executionId)
            { return armarx::skills::skillExecutionFailed(updates.at(executionId)); };
 
            const auto getSkillID = [](const armarx::skills::SkillExecutionID& executionId)
            { return executionId.skillId; };
 
            return ranges::views::keys(updates) | ranges::views::filter(failed) |
                   ranges::views::transform(getSkillID) | ranges::to_vector;
        }
 
        JoinAllResult
        joinAll(const std::vector<armarx::skills::SkillExecutionHandle*>& handles)
        {
            const auto join = [](armarx::skills::SkillExecutionHandle& handle)
            { return handle.join(); };
 
            const auto getExecutionID = [](const armarx::skills::SkillExecutionHandle& handle)
            { return handle.executionId(); };
 
            const auto executionIDs = handles | ranges::views::indirect |
                                      ranges::views::transform(getExecutionID) | ranges::to_vector;
            const auto updates = handles | ranges::views::indirect |
                                 ranges::views::transform(join) | ranges::to_vector;
 
            return JoinAllResult{ranges::views::zip(executionIDs, updates) |
                                 ranges::to<JoinAllResult::UpdatesMap>()};
        }
 
        JoinAllResult
        joinAll(const std::vector<std::unique_ptr<armarx::skills::SkillExecutionHandle>>& handles)
        {
            const std::vector<armarx::skills::SkillExecutionHandle*> rawHandles =
                handles | ranges::views::transform([](auto& p) { return p.get(); }) |
                ranges::to_vector;
 
            return static_cast<JoinAllResult (*)(const std::vector<SkillExecutionHandle*>&)>(
                joinAll)(rawHandles);
        }
 
        JoinAllResult
        joinAll(const std::vector<std::shared_ptr<armarx::skills::SkillExecutionHandle>>& handles)
        {
            const std::vector<armarx::skills::SkillExecutionHandle*> rawHandles =
                handles | ranges::views::transform([](auto& p) { return p.get(); }) |
                ranges::to_vector;
 
            return static_cast<JoinAllResult (*)(const std::vector<SkillExecutionHandle*>&)>(
                joinAll)(rawHandles);
        }
 
 
    } // namespace util
 
    const skills::SkillExecutionID&
    SkillExecutionHandle::executionId() const
    {
        return executionId_;
    }
} // namespace armarx::skills