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/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/factories/TrajectoryControllerFactory.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)
    {
        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 safetyGuardCfg = getElementOrNull(core::StackLayer::SafetyGuard);
 
        const safety_guard::SafetyGuard::Context ctx{.arviz = arviz};
 
        return server::NavigationStack{
            .globalPlanner = GlobalPlannerFactory::create(globalPlannerCfg, scene),
            .localPlanner = LocalPlannerFactory::create(localPlannerCfg, scene),
            .trajectoryControl = TrajectoryControllerFactory::create(trajectoryControllerCfg),
            .safetyGuard = SafetyGuardFactory::create(safetyGuardCfg, scene, ctx)};
    }
 
    server::NavigationStack
    NavigationStackFactory::create(const aron::data::dto::DictPtr& params,
                                   const core::Scene& scene,
                                   std::experimental::observer_ptr<viz::Client> arviz)
    {
        ARMARX_TRACE;
        const auto dict = aron::data::Dict::FromAronDictDTO(params);
        ARMARX_CHECK_NOT_NULL(dict);
        return create(dict, scene, arviz);
    }
 
} // namespace armarx::navigation::fac