NavigationStackFactory.cpp

Go to the documentation of this file.

#include "NavigationStackFactory.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/factories/GlobalPlannerFactory.h>
#include <armarx/navigation/factories/LocalPlannerFactory.h>
#include <armarx/navigation/factories/SafetyControllerFactory.h>
#include <armarx/navigation/factories/TrajectoryControllerFactory.h>
#include <armarx/navigation/server/NavigationStack.h>
 
 
namespace armarx::navigation::fac
{
    server::NavigationStack
    NavigationStackFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
    {
        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 '" << key << "'";
 
            return nullptr;
        };
 
        const auto globalPlannerCfg = getElementOrNull(core::StackLayer::GlobalPlanner);
        const auto localPlannerCfg = getElementOrNull(core::StackLayer::LocalPlanner);
        const auto trajectoryControllerCfg =
            getElementOrNull(core::StackLayer::TrajectoryController);
        const auto safetyControllerCfg = getElementOrNull(core::StackLayer::SafetyController);
 
        return server::NavigationStack{
            .globalPlanner = GlobalPlannerFactory::create(globalPlannerCfg, ctx),
            .localPlanner = LocalPlannerFactory::create(localPlannerCfg, ctx),
            .trajectoryControl = TrajectoryControllerFactory::create(trajectoryControllerCfg)};
            // .safetyControl = SafetyControllerFactory::create(safetyControllerCfg, ctx)};
    }
 
    server::NavigationStack
    NavigationStackFactory::create(const aron::data::dto::DictPtr& params, const core::Scene& ctx)
    {
        ARMARX_TRACE;
        const auto dict = aron::data::Dict::FromAronDictDTO(params);
        ARMARX_CHECK_NOT_NULL(dict);
        return create(dict, ctx);
    }
 
} // namespace armarx::navigation::fac