GlobalPlannerFactory.cpp

Go to the documentation of this file.

#include "GlobalPlannerFactory.h"
 
#include <memory>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
 
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/fwd.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/AStarWithOrientation.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/global_planning/core.h>
 
namespace armarx::navigation::fac
{
    global_planning::GlobalPlannerPtr
    GlobalPlannerFactory::create(const core::GeneralConfig& generalConfig,
                                 const aron::data::DictPtr& params,
                                 const core::Scene& ctx)
    {
        ARMARX_TRACE;
        namespace layer = global_planning;
 
        if (not params)
        {
            return nullptr;
        }
 
        // algo name
        const auto algoName = aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
        ARMARX_CHECK_NOT_NULL(algoName);
        const layer::Algorithms algo = layer::AlgorithmNames.from_name(algoName->getValue());
 
        // algo params
        const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
        ARMARX_CHECK_NOT_NULL(algoParams);
 
        global_planning::GlobalPlannerPtr globalPlanner;
        switch (algo)
        {
            case global_planning::Algorithms::AStar:
                ARMARX_INFO << "Using " << VAROUT("AStar") << " global planning strategy.";
                globalPlanner = std::make_shared<global_planning::AStar>(
                    global_planning::AStarParams::FromAron(algoParams),
                    generalConfig,
                    ctx);
                break;
            case global_planning::Algorithms::SPFA:
                ARMARX_INFO << "Using " << VAROUT("SPFA") << " global planning strategy.";
                globalPlanner = std::make_shared<global_planning::SPFA>(
                    global_planning::SPFAParams::FromAron(algoParams),
                    generalConfig,
                    ctx);
                break;
            case global_planning::Algorithms::Point2Point:
                ARMARX_INFO << "Using " << VAROUT("Point2Point") << " global planning strategy.";
                globalPlanner = std::make_shared<global_planning::Point2Point>(
                    global_planning::Point2PointParams::FromAron(algoParams),
                    generalConfig,
                    ctx);
                break;
            case global_planning::Algorithms::AStarWithOrientation:
                ARMARX_INFO << "Using " << VAROUT("AStarWithOrientation")
                            << " global planning strategy.";
                globalPlanner = std::make_shared<global_planning::AStarWithOrientation>(
                    global_planning::AStarWithOrientationParams::FromAron(algoParams),
                    generalConfig,
                    ctx);
        }
 
        return globalPlanner;
    }
 
} // namespace armarx::navigation::fac