NavigationStackFactory.cpp

Go to the documentation of this file.

#include "NavigationStackFactory.h"
 
#include <experimental/memory>
#include <string>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/GlobalPlannerFactory.h>
#include <armarx/navigation/factories/LocalPlannerFactory.h>
#include <armarx/navigation/factories/SafetyGuardFactory.h>
#include <armarx/navigation/safety_guard/SafetyGuard.h>
#include <armarx/navigation/server/NavigationStack.h>
 
namespace armarx::navigation::fac
{
    server::NavigationStack
    NavigationStackFactory::create(
        const aron::data::DictPtr& params,
        const core::Scene& scene,
        std::experimental::observer_ptr<viz::Client> arviz,
        std::experimental::observer_ptr<DebugObserverComponentPluginUser> debugObserver)
    {
        ARMARX_TRACE;
        using aron::data::Dict;
 
        const auto getElementOrNull = [&params](const core::StackLayer& layer) -> Dict::PointerType
        {
            ARMARX_TRACE;
            const std::string key = core::StackLayerNames.to_name(layer);
 
            if (params->hasElement(key))
            {
                return Dict::DynamicCast(params->getElement(key));
            }
 
            ARMARX_INFO << "Skipping " << QUOTED(key);
 
            return nullptr;
        };
 
        const auto generalConfigDict = getElementOrNull(core::StackLayer::GeneralConfig);
        ARMARX_CHECK_NOT_NULL(generalConfigDict);
        const core::GeneralConfig generalConfig = core::GeneralConfig::FromAron(
            aron::data::Dict::DynamicCast(generalConfigDict->getElement(core::PARAMS_KEY)));
 
        const auto globalPlannerCfg = getElementOrNull(core::StackLayer::GlobalPlanner);
        const auto localPlannerCfg = getElementOrNull(core::StackLayer::LocalPlanner);
        const auto safetyGuardCfg = getElementOrNull(core::StackLayer::SafetyGuard);
 
        const safety_guard::SafetyGuard::Context ctx{.arviz = arviz,
                                                     .debugObserver = debugObserver};
 
        return server::NavigationStack{
            .generalConfig = generalConfig,
            .globalPlanner = GlobalPlannerFactory::create(generalConfig, globalPlannerCfg, scene),
            .localPlanner = LocalPlannerFactory::create(generalConfig, localPlannerCfg, scene),
            .safetyGuard = SafetyGuardFactory::create(generalConfig, safetyGuardCfg, scene, ctx)};
    }
 
    server::NavigationStack
    NavigationStackFactory::create(
        const aron::data::dto::DictPtr& params,
        const core::Scene& scene,
        std::experimental::observer_ptr<viz::Client> arviz,
        std::experimental::observer_ptr<DebugObserverComponentPluginUser> debugObserver)
    {
        ARMARX_TRACE;
        const auto dict = aron::data::Dict::FromAronDictDTO(params);
        ARMARX_CHECK_NOT_NULL(dict);
        return create(dict, scene, arviz, debugObserver);
    }
 
} // namespace armarx::navigation::fac