Prediction.h

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 flied 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::armem::core::base::detail
 * @author     phesch ( phesch at student dot kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <functional>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
#include <RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h>
#include <RobotAPI/libraries/armem/core/base/detail/Predictive.h>
#include <RobotAPI/libraries/armem/core/container_maps.h>
 
 
namespace armarx::armem::server::wm::detail
{
 
    using Predictor = std::function<PredictionResult(const PredictionRequest&)>;
 
    /**
     * Can do predictions, but has no children it could delegate predictions to.
     *
     * This class is integrated with `armem::base::detail::Predictive`:
     * If `DerivedT` is also a `Predictive`, the setters of this class also update the
     * `Predictive` part.
     */
    template <class DerivedT>
    class Prediction
    {
        using Predictive = armem::base::detail::Predictive<DerivedT>;
 
        Predictive* _asPredictive()
        {
            return dynamic_cast<Predictive*>(&base::detail::derived<DerivedT>(this));
        }
 
    public:
        explicit Prediction(const std::map<PredictionEngine, Predictor>& predictors = {})
        {
            this->setPredictors(predictors);
        }
 
        void
        addPredictor(const PredictionEngine& engine, Predictor&& predictor)
        {
            _predictors.emplace(engine.engineID, predictor);
 
            if (Predictive* predictive = this->_asPredictive())
            {
                predictive->addPredictionEngine(engine);
            }
        }
 
        void
        setPredictors(const std::map<PredictionEngine, Predictor>& predictors)
        {
            this->_predictors.clear();
            for (const auto& [engine, predictor] : predictors)
            {
                _predictors.emplace(engine.engineID, predictor);
            }
 
            if (Predictive* predictive = this->_asPredictive())
            {
                predictive->setPredictionEngines(simox::alg::get_keys(predictors));
            }
        }
 
        /**
         * Resolves mapping of requests to predictors and dispatches them.
         *
         * In this case, the resolution is basically no-op because there are no children.
         */
        std::vector<PredictionResult>
        dispatchPredictions(const std::vector<PredictionRequest>& requests)
        {
            const MemoryID ownID = base::detail::derived<DerivedT>(this).id();
            std::vector<PredictionResult> results;
            for (const auto& request : requests)
            {
                results.push_back(dispatchTargetedPrediction(request, ownID));
            }
            return results;
        }
 
        /**
         * Dispatches a single prediction request (assuming resolution was done by the caller).
         */
        PredictionResult
        dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
        {
            PredictionResult result;
            result.snapshotID = request.snapshotID;
 
            MemoryID ownID = base::detail::derived<DerivedT>(this).id();
            if (ownID == target)
            {
                auto it = _predictors.find(request.predictionSettings.predictionEngineID);
                if (it != _predictors.end())
                {
                    const Predictor& predictor = it->second;
                    result = predictor(request);
                }
                else
                {
                    result.success = false;
                    std::stringstream sstream;
                    sstream << "Could not dispatch prediction request for " << request.snapshotID
                            << " with engine '" << request.predictionSettings.predictionEngineID
                            << "' in " << ownID << ": Engine not registered.";
                    result.errorMessage = sstream.str();
                }
            }
            else
            {
                result.success = false;
                std::stringstream sstream;
                sstream << "Could not dispatch prediction request for " << request.snapshotID
                        << " to " << target << " from " << ownID;
                result.errorMessage = sstream.str();
            }
            return result;
        }
 
    private:
        std::map<std::string, Predictor> _predictors; // NOLINT
    };
 
 
    /**
     * Can do predictions itself and has children it could delegate predictions to.
     */
    template <class DerivedT>
    class PredictionContainer : public Prediction<DerivedT>
    {
    public:
        using Prediction<DerivedT>::Prediction;
 
        explicit PredictionContainer(const std::map<PredictionEngine, Predictor>& predictors = {})
            : Prediction<DerivedT>(predictors)
        {
        }
 
        std::vector<PredictionResult>
        dispatchPredictions(const std::vector<PredictionRequest>& requests)
        {
            const auto& derivedThis = base::detail::derived<DerivedT>(this);
            const std::map<MemoryID, std::vector<PredictionEngine>> engines =
                derivedThis.getAllPredictionEngines();
 
            std::vector<PredictionResult> results;
            for (const PredictionRequest& request : requests)
            {
                PredictionResult& result = results.emplace_back();
                result.snapshotID = request.snapshotID;
 
                auto iter =
                    armem::findMostSpecificEntryContainingIDAnd<std::vector<PredictionEngine>>(
                        engines, request.snapshotID,
                        [&request](const MemoryID& /*unused*/,
                                   const std::vector<PredictionEngine>& supported) -> bool
                        {
                            for (const PredictionEngine& engine : supported)
                            {
                                if (engine.engineID == request.predictionSettings.predictionEngineID)
                                {
                                    return true;
                                }
                            }
                            return false;
                        });
 
                if (iter != engines.end())
                {
                    const MemoryID& responsibleID = iter->first;
 
                    result = dispatchTargetedPrediction(request, responsibleID);
                }
                else
                {
                    result.success = false;
                    std::stringstream sstream;
                    sstream << "Could not find segment offering prediction engine '"
                            << request.predictionSettings.predictionEngineID << "' for memory ID "
                            << request.snapshotID << ".";
                    result.errorMessage = sstream.str();
                }
            }
            return results;
        }
 
 
        /**
         * Semantics: This container or one of its children (target) is responsible
         * for performing the prediction.
         */
        PredictionResult
        dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
        {
            PredictionResult result;
            result.snapshotID = request.snapshotID;
 
            const auto& derivedThis = base::detail::derived<DerivedT>(this);
            MemoryID ownID = derivedThis.id();
            if (ownID == target)
            {
                // Delegate to base class.
                result = Prediction<DerivedT>::dispatchTargetedPrediction(request, target);
            }
            // Check if of this' children is really responsible for the request.
            else if (contains(ownID, target))
            {
                std::string childName = _getChildName(ownID, target);
 
                // TODO(phesch): Looping over all the children just to find the one
                //               with the right name isn't nice, but it's the interface we've got.
                // TODO(RainerKartmann): Try to add findChild() to loopup mixins.
                typename DerivedT::ChildT* child = nullptr;
                derivedThis.forEachChild(
                    [&child, &childName](auto& otherChild)
                    {
                        if (otherChild.name() == childName)
                        {
                            child = &otherChild;
                        }
                    });
                if (child)
                {
                    result = child->dispatchTargetedPrediction(request, target);
                }
                else
                {
                    result.success = false;
                    std::stringstream sstream;
                    sstream << "Could not find memory item with ID " << target << ".";
                    result.errorMessage = sstream.str();
                }
            }
            else
            {
                result.success = false;
                std::stringstream sstream;
                sstream << "Could not dispatch prediction request for " << request.snapshotID
                        << " to " << target << " from " << ownID << ".";
                result.errorMessage = sstream.str();
            }
            return result;
        }
 
    private:
 
        std::string _getChildName(const MemoryID& parent, const MemoryID& child)
        {
            ARMARX_CHECK(armem::contains(parent, child));
            ARMARX_CHECK(parent != child);
 
            size_t parentLength = parent.getItems().size();
 
            // Get iterator to first entry of child ID (memory).
            std::vector<std::string> childItems = child.getItems();
 
            int index = parentLength;
            ARMARX_CHECK_FITS_SIZE(index, childItems.size());
 
            return childItems[index];
        }
 
 
    };
 
} // namespace armarx::armem::server::wm::detail