MemoryParameterizationService.cpp

Go to the documentation of this file.

#include "MemoryParameterizationService.h"
 
#include <string>
#include <unordered_map>
 
#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/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/memory/client/parameterization/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Writer.h>
 
namespace armarx::navigation::server
{
    bool
    MemoryParameterizationService::store(const aron::data::DictPtr& params,
                                         const std::string& clientId,
                                         const armarx::core::time::DateTime& timestamp)
    {
        ARMARX_CHECK_NOT_NULL(writer);
 
        using aron::data::Dict;
 
        const auto getElementOrNull = [&params](const core::StackLayer& layer) -> Dict::PointerType
        {
            const std::string key = core::StackLayerNames.to_name(layer);
 
            if (params->hasElement(key))
            {
                return Dict::DynamicCast(params->getElement(key));
            }
 
            ARMARX_INFO << "Skipping '" << key << "'";
 
            return nullptr;
        };
 
        // convert to more structured representation
        std::unordered_map<core::StackLayer, Dict::PointerType> cfgs;
 
        if (const auto globalPlannerCfg = getElementOrNull(core::StackLayer::GlobalPlanner))
        {
            cfgs.emplace(core::StackLayer::GlobalPlanner, globalPlannerCfg);
        }
 
        if (const auto localPlannerCfg = getElementOrNull(core::StackLayer::LocalPlanner))
        {
            cfgs.emplace(core::StackLayer::LocalPlanner, localPlannerCfg);
        }
 
        if (const auto trajectoryControllerCfg =
                getElementOrNull(core::StackLayer::TrajectoryController))
        {
            cfgs.emplace(core::StackLayer::TrajectoryController, trajectoryControllerCfg);
        }
 
        if (const auto safetyControllerCfg = getElementOrNull(core::StackLayer::SafetyGuard))
        {
            cfgs.emplace(core::StackLayer::SafetyGuard, safetyControllerCfg);
        }
 
        return writer->store(cfgs, clientId, timestamp);
    }
 
    bool
    MemoryParameterizationService::store(const aron::data::dto::DictPtr& params,
                                         const std::string& clientId,
                                         const armarx::core::time::DateTime& timestamp)
    {
        const auto dict = aron::data::Dict::FromAronDictDTO(params);
        return store(dict, clientId, timestamp);
    }
 
    MemoryParameterizationService::MemoryParameterizationService(
        memory::client::param::Reader* reader,
        memory::client::param::Writer* writer) :
        reader(reader), writer(writer)
    {
    }
} // namespace armarx::navigation::server