NavigationStackConfig.cpp

Go to the documentation of this file.

#include "NavigationStackConfig.h"
 
#include <memory>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/interface/aron/Aron.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/constants.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/core.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/core.h>
#include <armarx/navigation/safety_guard/SafetyGuard.h>
#include <armarx/navigation/safety_guard/core.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
#include <armarx/navigation/trajectory_control/local/core.h>
 
namespace armarx::navigation::client
{
 
    aron::data::dto::DictPtr
    NavigationStackConfig::toAron() const
    {
        ARMARX_CHECK(isValid())
            << "The NavigationStackConfig is not valid as some elements are not set!";
        return dict.toAronDictDTO();
    }
 
    bool
    NavigationStackConfig::isValid() const
    {
        if (not dict.hasElement(core::StackLayerNames.to_name(core::StackLayer::GeneralConfig)))
        {
            ARMARX_WARNING << "General params not set!";
            return false;
        }
 
        if (not dict.hasElement(core::StackLayerNames.to_name(core::StackLayer::GlobalPlanner)))
        {
            ARMARX_WARNING << "Global planner not set!";
            return false;
        }
 
        if (dict.hasElement(core::StackLayerNames.to_name(core::StackLayer::LocalPlanner)) and
            dict.hasElement(core::StackLayerNames.to_name(core::StackLayer::SafetyGuard)))
        {
            ARMARX_WARNING << "Safety guard is not supported for local planner!";
            return false;
        }
 
        return true;
    }
 
    NavigationStackConfig&
    NavigationStackConfig::general(const core::GeneralConfig& cfg)
    {
        aron::data::DictPtr element(new aron::data::Dict);
        element->setElementCopy(core::NAME_KEY,
                                std::make_shared<aron::data::String>("GeneralConfig"));
        element->setElementCopy(core::PARAMS_KEY, cfg.toAron());
 
        dict.setElementCopy(core::StackLayerNames.to_name(core::StackLayer::GeneralConfig),
                            element);
 
        return *this;
    }
 
    NavigationStackConfig&
    NavigationStackConfig::globalPlanner(const global_planning::GlobalPlannerParams& params)
    {
        aron::data::DictPtr element(new aron::data::Dict);
        element->setElementCopy(core::NAME_KEY,
                                std::make_shared<aron::data::String>(
                                    global_planning::AlgorithmNames.to_name(params.algorithm())));
        element->setElementCopy(core::PARAMS_KEY, params.toAron());
 
        dict.setElementCopy(core::StackLayerNames.to_name(core::StackLayer::GlobalPlanner),
                            element);
 
        return *this;
    }
 
    NavigationStackConfig&
    NavigationStackConfig::localPlanner(const local_planning::LocalPlannerParams& params)
    {
        aron::data::DictPtr element(new aron::data::Dict);
        element->setElementCopy(core::NAME_KEY,
                                std::make_shared<aron::data::String>(
                                    local_planning::AlgorithmNames.to_name(params.algorithm())));
        element->setElementCopy(core::PARAMS_KEY, params.toAron());
 
        dict.setElementCopy(core::StackLayerNames.to_name(core::StackLayer::LocalPlanner), element);
 
        return *this;
    }
 
    NavigationStackConfig&
    NavigationStackConfig::safetyGuard(const safety_guard::SafetyGuardParams& params)
    {
        aron::data::DictPtr element(new aron::data::Dict);
        element->setElementCopy(core::NAME_KEY,
                                std::make_shared<aron::data::String>(
                                    safety_guard::AlgorithmNames.to_name(params.algorithm())));
        element->setElementCopy(core::PARAMS_KEY, params.toAron());
 
        dict.setElementCopy(core::StackLayerNames.to_name(core::StackLayer::SafetyGuard), element);
 
        return *this;
    }
 
    NavigationStackConfig&
    NavigationStackConfig::goalReached(const core::GoalReachedConfig& cfg)
    {
        aron::data::DictPtr element(new aron::data::Dict);
        element->setElementCopy(core::NAME_KEY,
                                std::make_shared<aron::data::String>("GoalReachedConfig"));
        element->setElementCopy(core::PARAMS_KEY, cfg.toAron());
 
        dict.setElementCopy(core::StackLayerNames.to_name(core::StackLayer::GoalReachedConfig),
                            element);
 
        return *this;
    }
} // namespace armarx::navigation::client