NavigationStackGeneralConfig.cpp

Go to the documentation of this file.

#include "NavigationStackGeneralConfig.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/Int.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
 
#include <armarx/navigation/core/aron/GeneralConfig.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
 
namespace armarx::navigation::core
{
 
    std::string
    ToString(const TrajectoryParametrization parametrization)
    {
        switch (parametrization)
        {
            case TrajectoryParametrization::None:
                return "none";
            case TrajectoryParametrization::Ramping:
                return "ramping";
            case TrajectoryParametrization::Toppra:
                return "toppra";
            default:
                return "unknown";
        }
    }
 
    aron::data::DictPtr
    GeneralConfig::toAron() const
    {
        arondto::GeneralConfig dto;
 
        ::armarx::navigation::core::toAron(dto, *this);
 
        return dto.toAron();
    }
 
    GeneralConfig
    GeneralConfig::FromAron(const aron::data::DictPtr& dict)
    {
        ARMARX_CHECK_NOT_NULL(dict);
 
        // `parametrization` was added after this config had already been deployed, and the
        // generated reader throws on a member it cannot find rather than defaulting. A config
        // serialized by an older client therefore has to be completed here.
        //
        // Ramping, not the enum's zero value: these configs predate the choice existing, and
        // ramping is unconditionally what the navigator did for them. Defaulting to `None`
        // would silently disable it on every robot still sending an old config.
        if (not dict->hasElement("parametrization"))
        {
            dict->setElementCopy(
                "parametrization",
                std::make_shared<aron::data::Int>(arondto::TrajectoryParametrization::EnumToValueMap.at(
                    arondto::TrajectoryParametrization::RAMPING)));
        }
 
        arondto::GeneralConfig dto;
        dto.fromAron(dict);
 
        GeneralConfig bo;
        fromAron(dto, bo);
 
        return bo;
    }
} // namespace armarx::navigation::core