CHolonomicND.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "nav-precomp.h"  // Precomp header

#include <mrpt/nav/holonomic/CHolonomicND.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/core/round.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::nav;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CLogFileRecord_ND, CHolonomicLogFileRecord, mrpt::nav)
IMPLEMENTS_SERIALIZABLE(
    CHolonomicND, CAbstractHolonomicReactiveMethod, mrpt::nav)

/**  Initialize the parameters of the navigator, from some
*    configuration file, or default values if filename is set to NULL.
*/
CHolonomicND::CHolonomicND(const mrpt::config::CConfigFileBase* INI_FILE)
    : CAbstractHolonomicReactiveMethod("CHolonomicND"),
      m_last_selected_sector(std::numeric_limits<unsigned int>::max())
{
    if (INI_FILE != nullptr) initialize(*INI_FILE);
}

void CHolonomicND::initialize(const mrpt::config::CConfigFileBase& INI_FILE)
{
    options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
}
void CHolonomicND::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
    options.saveToConfigFile(c, getConfigFileSectionName());
}

/*---------------------------------------------------------------
                        Navigate
  ---------------------------------------------------------------*/
void CHolonomicND::navigate(const NavInput& ni, NavOutput& no)
{
    const auto ptg = getAssociatedPTG();
    const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;

    TGapArray gaps;
    TSituations situation;
    unsigned int selectedSector;
    double riskEvaluation;
    double evaluation;

    // Create a log record for returning data.
    CLogFileRecord_ND::Ptr log = mrpt::make_aligned_shared<CLogFileRecord_ND>();
    no.logRecord = log;

    // Search gaps:
    gaps.clear();
    ASSERT_(!ni.targets.empty());
    const auto trg = *ni.targets.rbegin();

    gapsEstimator(ni.obstacles, trg, gaps);

    // Select best gap:
    searchBestGap(
        ni.obstacles, 1.0 /* max obs range*/, gaps, trg, selectedSector,
        evaluation, situation, riskEvaluation, *log);

    if (situation == SITUATION_NO_WAY_FOUND)
    {
        // No way found!
        no.desiredDirection = 0;
        no.desiredSpeed = 0;
    }
    else
    {
        // A valid movement:
        no.desiredDirection = CParameterizedTrajectoryGenerator::index2alpha(
            selectedSector, ni.obstacles.size());

        // Speed control: Reduction factors
        // ---------------------------------------------
        const double targetNearnessFactor =
            m_enableApproachTargetSlowDown
                ? std::min(
                      1.0,
                      trg.norm() / (options.TARGET_SLOW_APPROACHING_DISTANCE /
                                    ptg_ref_dist))
                : 1.0;

        const double riskFactor =
            std::min(1.0, riskEvaluation / options.RISK_EVALUATION_DISTANCE);
        no.desiredSpeed =
            ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
    }

    m_last_selected_sector = selectedSector;

    // LOG --------------------------
    if (log)
    {
        // gaps:
        {
            int i, n = gaps.size();
            log->gaps_ini.resize(n);
            log->gaps_end.resize(n);
            for (i = 0; i < n; i++)
            {
                log->gaps_ini[i] = gaps[i].ini;
                log->gaps_end[i] = gaps[i].end;
            }
        }
        // Selection:
        log->selectedSector = selectedSector;
        log->evaluation = evaluation;
        log->situation = situation;
        log->riskEvaluation = riskEvaluation;
    }
}

/*---------------------------------------------------------------
                Find gaps in the obtacles (Beta version)
  ---------------------------------------------------------------*/
void CHolonomicND::gapsEstimator(
    const std::vector<double>& obstacles, const mrpt::math::TPoint2D& target,
    TGapArray& gaps_out)
{
    const size_t n = obstacles.size();
    ASSERT_(n > 2);

    // ================ Parameters ================
    const int GAPS_MIN_WIDTH = ceil(n * 0.01);  // was: 3
    const double GAPS_MIN_DEPTH_CONSIDERED = 0.6;
    const double GAPS_MAX_RELATIVE_DEPTH = 0.5;
    // ============================================

    // Find the maximum distances to obstacles:
    // ----------------------------------------------------------
    float overall_max_dist = std::numeric_limits<float>::min(),
          overall_min_dist = std::numeric_limits<float>::max();
    for (size_t i = 1; i < (n - 1); i++)
    {
        mrpt::keep_max(overall_max_dist, obstacles[i]);
        mrpt::keep_min(overall_min_dist, obstacles[i]);
    }
    double max_depth = overall_max_dist - overall_min_dist;

    //  Build list of "GAPS":
    // --------------------------------------------------------
    TGapArray gaps_temp;
    gaps_temp.reserve(150);

    for (double threshold_ratio = 0.95; threshold_ratio >= 0.05;
         threshold_ratio -= 0.05)
    {
        const double dist_threshold =
            threshold_ratio * overall_max_dist +
            (1.0f - threshold_ratio) *
                min(target.norm(), GAPS_MIN_DEPTH_CONSIDERED);

        bool is_inside = false;
        size_t sec_ini = 0, sec_end = 0;
        double maxDist = 0.;

        for (size_t i = 0; i < n; i++)
        {
            if (!is_inside && (obstacles[i] >= dist_threshold))  // A gap begins
            {
                sec_ini = i;
                maxDist = obstacles[i];
                is_inside = true;
            }
            else if (
                is_inside &&
                (i == (n - 1) || obstacles[i] < dist_threshold))  // A gap ends
            {
                if (obstacles[i] < dist_threshold)
                    sec_end = i - 1;
                else
                    sec_end = i;

                is_inside = false;

                if ((sec_end - sec_ini) >= (size_t)GAPS_MIN_WIDTH)
                {
                    // Add new gap:
                    gaps_temp.resize(gaps_temp.size() + 1);
                    TGap& newGap = *gaps_temp.rbegin();

                    newGap.ini = sec_ini;
                    newGap.end = sec_end;
                    newGap.minDistance =
                        min(obstacles[sec_ini], obstacles[sec_end]);
                    newGap.maxDistance = maxDist;
                }
            }

            if (is_inside) maxDist = std::max(maxDist, obstacles[i]);
        }
    }

    // Start to filter the gap list
    //--------------------------------------------------------------

    const size_t nTempGaps = gaps_temp.size();

    std::vector<bool> delete_gaps;
    delete_gaps.assign(nTempGaps, false);

    // First, remove redundant gaps
    for (size_t i = 0; i < nTempGaps; i++)
    {
        if (delete_gaps[i] == 1) continue;

        for (size_t j = i + 1; j < nTempGaps; j++)
        {
            if (gaps_temp[i].ini == gaps_temp[j].ini ||
                gaps_temp[i].end == gaps_temp[j].end)
                delete_gaps[j] = 1;
        }
    }

    // Remove gaps with a big depth
    for (size_t i = 0; i < nTempGaps; i++)
    {
        if (delete_gaps[i] == 1) continue;

        if ((gaps_temp[i].maxDistance - gaps_temp[i].minDistance) >
            max_depth * GAPS_MAX_RELATIVE_DEPTH)
            delete_gaps[i] = 1;
    }

    // Delete gaps which contain more than one other gaps
    for (size_t i = 0; i < nTempGaps; i++)
    {
        if (delete_gaps[i]) continue;

        unsigned int inner_gap_count = 0;

        for (unsigned int j = 0; j < nTempGaps; j++)
        {
            if (i == j || delete_gaps[j]) continue;

            // j is inside of i?
            if (gaps_temp[j].ini >= gaps_temp[i].ini &&
                gaps_temp[j].end <= gaps_temp[i].end)
                if (++inner_gap_count > 1)
                {
                    delete_gaps[i] = 1;
                    break;
                }
        }
    }

    // Delete gaps included in other gaps
    for (size_t i = 0; i < nTempGaps; i++)
    {
        if (delete_gaps[i]) continue;

        for (unsigned int j = 0; j < nTempGaps; j++)
        {
            if (i == j || delete_gaps[j]) continue;
            if (gaps_temp[i].ini <= gaps_temp[j].ini &&
                gaps_temp[i].end >= gaps_temp[j].end)
                delete_gaps[j] = 1;
        }
    }

    // Copy as result only those gaps not marked for deletion:
    // --------------------------------------------------------
    gaps_out.clear();
    gaps_out.reserve(nTempGaps / 2);
    for (size_t i = 0; i < nTempGaps; i++)
    {
        if (delete_gaps[i]) continue;

        // Compute the representative direction ("sector") for this gap:
        calcRepresentativeSectorForGap(gaps_temp[i], target, obstacles);

        gaps_out.push_back(gaps_temp[i]);
    }
}

/*---------------------------------------------------------------
                        Search the best gap.
  ---------------------------------------------------------------*/
void CHolonomicND::searchBestGap(
    const std::vector<double>& obstacles, const double maxObsRange,
    const TGapArray& in_gaps, const mrpt::math::TPoint2D& target,
    unsigned int& out_selDirection, double& out_selEvaluation,
    TSituations& out_situation, double& out_riskEvaluation,
    CLogFileRecord_ND& log)
{
    // For evaluating the "risk":
    unsigned int min_risk_eval_sector = 0;
    unsigned int max_risk_eval_sector = obstacles.size() - 1;
    const unsigned int target_sector =
        direction2sector(atan2(target.y, target.x), obstacles.size());
    const double target_dist = std::max(0.01, target.norm());
    // (Risk is evaluated at the end, for all the situations)

    // D1 : Straight path?
    // --------------------------------------------------------
    const int freeSectorsNearTarget = ceil(0.02 * obstacles.size());
    bool theyAreFree = true, caseD1 = false;
    if (target_sector > static_cast<unsigned int>(freeSectorsNearTarget) &&
        target_sector <
            static_cast<unsigned int>(obstacles.size() - freeSectorsNearTarget))
    {
        const double min_free_dist =
            std::min(1.05 * target_dist, 0.95 * maxObsRange);
        for (int j = -freeSectorsNearTarget;
             theyAreFree && j <= freeSectorsNearTarget; j++)
            if (obstacles[(int(target_sector) + j) % obstacles.size()] <
                min_free_dist)
                theyAreFree = false;
        caseD1 = theyAreFree;
    }

    if (caseD1)
    {
        // S1: Move straight towards target:
        out_selDirection = target_sector;

        // In case of several paths, the shortest:
        out_selEvaluation =
            1.0 + std::max(0.0, (maxObsRange - target_dist) / maxObsRange);
        out_situation = SITUATION_TARGET_DIRECTLY;
    }
    else
    {
        // Evaluate all gaps (if any):
        std::vector<double> gaps_evaluation;
        int selected_gap = -1;
        double selected_gap_eval = -100;

        evaluateGaps(
            obstacles, maxObsRange, in_gaps, target_sector, target_dist,
            gaps_evaluation);

        log.gaps_eval = gaps_evaluation;

        // D2: is there any gap "beyond" the target (and not too far away)?
        // (Not used)
        // ----------------------------------------------------------------

        // unsigned int dist;
        // for ( unsigned int i=0;i<in_gaps.size();i++ )
        //{
        //  dist = mrpt::abs_diff(target_sector,
        // in_gaps[i].representative_sector );
        //  if (dist > 0.5*obstacles.size())
        //      dist = obstacles.size() - dist;
        //
        //  if ( in_gaps[i].maxDistance >= target_dist && dist <=
        //(int)floor(options.MAX_SECTOR_DIST_FOR_D2_PERCENT * obstacles.size())
        //)
        //
        //      if ( gaps_evaluation[i]>selected_gap_eval )
        //      {
        //          selected_gap_eval = gaps_evaluation[i];
        //          selected_gap = i;
        //      }
        //}

        // Keep the best gaps (if none was picked up to this point)
        if (selected_gap == -1)
            for (unsigned int i = 0; i < in_gaps.size(); i++)
                if (gaps_evaluation[i] > selected_gap_eval)
                {
                    selected_gap_eval = gaps_evaluation[i];
                    selected_gap = i;
                }

        //  D3: Wasn't a good enough gap (or there were none)?
        // ----------------------------------------------------------
        if (selected_gap_eval <= 0)
        {
            // S2: No way found
            // ------------------------------------------------------
            out_selDirection = 0;
            out_selEvaluation = 0.0;  // Worst case
            out_situation = SITUATION_NO_WAY_FOUND;
        }
        else
        {
            // The selected gap:
            const TGap& gap = in_gaps[selected_gap];

            const unsigned int sectors_to_be_wide =
                round(options.WIDE_GAP_SIZE_PERCENT * obstacles.size());

            out_selDirection = in_gaps[selected_gap].representative_sector;
            out_selEvaluation = selected_gap_eval;

            // D4: Is it a WIDE gap?
            // -----------------------------------------------------
            if ((gap.end - gap.ini) < sectors_to_be_wide)
            {
                // S3: Narrow gap
                // -------------------------------------------
                out_situation = SITUATION_SMALL_GAP;
            }
            else
            {
                // S4: Wide gap
                // -------------------------------------------
                out_situation = SITUATION_WIDE_GAP;
            }

            // Evaluate the risk only within the gap:
            min_risk_eval_sector = gap.ini;
            max_risk_eval_sector = gap.end;
        }
    }

    // Evaluate short-term minimum distance to obstacles, in a small interval
    // around the selected direction:
    const unsigned int risk_eval_nsectors =
        round(options.RISK_EVALUATION_SECTORS_PERCENT * obstacles.size());
    const unsigned int sec_ini = std::max(
        min_risk_eval_sector, risk_eval_nsectors < out_selDirection
                                  ? out_selDirection - risk_eval_nsectors
                                  : 0);
    const unsigned int sec_fin =
        std::min(max_risk_eval_sector, out_selDirection + risk_eval_nsectors);

    out_riskEvaluation = 0.0;
    for (unsigned int i = sec_ini; i <= sec_fin; i++)
        out_riskEvaluation += obstacles[i];
    out_riskEvaluation /= (sec_fin - sec_ini + 1);
}

/*---------------------------------------------------------------
    Fills in the representative sector
        field in the gap structure:
  ---------------------------------------------------------------*/
void CHolonomicND::calcRepresentativeSectorForGap(
    TGap& gap, const mrpt::math::TPoint2D& target,
    const std::vector<double>& obstacles)
{
    int sector;
    const unsigned int sectors_to_be_wide =
        round(options.WIDE_GAP_SIZE_PERCENT * obstacles.size());
    const unsigned int target_sector =
        direction2sector(atan2(target.y, target.x), obstacles.size());

    if ((gap.end - gap.ini) <
        sectors_to_be_wide)  // Select the intermediate sector
    {
#if 1
        sector = round(0.5f * gap.ini + 0.5f * gap.end);
#else
        float min_dist_obs_near_ini = 1, min_dist_obs_near_end = 1;
        int i;
        for (i = gap.ini; i >= max(0, gap.ini - 2); i--)
            min_dist_obs_near_ini = min(min_dist_obs_near_ini, obstacles[i]);
        for (i = gap.end; i <= min((int)obstacles.size() - 1, gap.end + 2); i++)
            min_dist_obs_near_end = min(min_dist_obs_near_end, obstacles[i]);
        sector = round(
            (min_dist_obs_near_ini * gap.ini +
             min_dist_obs_near_end * gap.end) /
            (min_dist_obs_near_ini + min_dist_obs_near_end));
#endif
    }
    else  // Select a sector close to the target but spaced
    // "sectors_to_be_wide/2" from it
    {
        unsigned int dist_ini = mrpt::abs_diff(target_sector, gap.ini);
        unsigned int dist_end = mrpt::abs_diff(target_sector, gap.end);

        if (dist_ini > 0.5 * obstacles.size())
            dist_ini = obstacles.size() - dist_ini;
        if (dist_end > 0.5 * obstacles.size())
            dist_end = obstacles.size() - dist_end;

        int dir;
        if (dist_ini < dist_end)
        {
            sector = gap.ini;
            dir = +1;
        }
        else
        {
            sector = gap.end;
            dir = -1;
        }

        sector = sector + dir * static_cast<int>(sectors_to_be_wide) / 2;
    }

    keep_max(sector, 0);
    keep_min(sector, static_cast<int>(obstacles.size()) - 1);

    gap.representative_sector = sector;
}

/*---------------------------------------------------------------
                        Evaluate each gap
  ---------------------------------------------------------------*/
void CHolonomicND::evaluateGaps(
    const std::vector<double>& obstacles, const double maxObsRange,
    const TGapArray& gaps, const unsigned int target_sector,
    const float target_dist, std::vector<double>& out_gaps_evaluation)
{
    out_gaps_evaluation.resize(gaps.size());

    const double targetAng = CParameterizedTrajectoryGenerator::index2alpha(
        target_sector, obstacles.size());
    const double target_x = target_dist * cos(targetAng);
    const double target_y = target_dist * sin(targetAng);

    for (unsigned int i = 0; i < gaps.size(); i++)
    {
        // Short cut:
        const TGap* gap = &gaps[i];

        const float d = min3(
            obstacles[gap->representative_sector], maxObsRange,
            0.95 * target_dist);

        // The TP-Space representative coordinates for this gap:
        const double phi = CParameterizedTrajectoryGenerator::index2alpha(
            gap->representative_sector, obstacles.size());
        const double x = d * cos(phi);
        const double y = d * sin(phi);

        // Factor #1: Maximum reachable distance with this PTG:
        // -----------------------------------------------------
        // It computes the average free distance of the gap:
        float meanDist = 0.f;
        for (unsigned int j = gap->ini; j <= gap->end; j++)
            meanDist += obstacles[j];
        meanDist /= (gap->end - gap->ini + 1);

        double factor1;
        if (mrpt::abs_diff(gap->representative_sector, target_sector) <= 1 &&
            target_dist < 1)
            factor1 = std::min(target_dist, meanDist) / target_dist;
        else
            factor1 = meanDist;

        // Factor #2: Distance to target in "sectors"
        // -------------------------------------------
        unsigned int dif =
            mrpt::abs_diff(target_sector, gap->representative_sector);

        // Handle the -PI,PI circular topology:
        if (dif > 0.5 * obstacles.size()) dif = obstacles.size() - dif;

        const double factor2 = exp(-square(dif / (obstacles.size() * 0.25)));

        // Factor #3: Punish paths that take us far away wrt the target:  **** I
        // don't understand it *********
        // -----------------------------------------------------
        double closestX, closestY;
        double dist_eucl = math::minimumDistanceFromPointToSegment(
            target_x, target_y,  // Point
            0, 0, x, y,  // Segment
            closestX, closestY  // Out
            );

        const float factor3 =
            (maxObsRange - std::min(maxObsRange, dist_eucl)) / maxObsRange;

        // Factor #4: Stabilizing factor (hysteresis) to avoid quick switch
        // among very similar paths:
        // ------------------------------------------------------------------------------------------
        double factor_AntiCab;

        if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
        {
            unsigned int dist = mrpt::abs_diff(
                m_last_selected_sector, gap->representative_sector);

            if (dist > unsigned(0.1 * obstacles.size()))
                factor_AntiCab = 0.0;
            else
                factor_AntiCab = 1.0;
        }
        else
        {
            factor_AntiCab = 0;
        }

        ASSERT_(options.factorWeights.size() == 4);

        if (obstacles[gap->representative_sector] <
            options.TOO_CLOSE_OBSTACLE)  // Too close to obstacles
            out_gaps_evaluation[i] = 0;
        else
            out_gaps_evaluation[i] =
                (options.factorWeights[0] * factor1 +
                 options.factorWeights[1] * factor2 +
                 options.factorWeights[2] * factor3 +
                 options.factorWeights[3] * factor_AntiCab) /
                (math::sum(options.factorWeights));
    }  // for each gap
}

unsigned int CHolonomicND::direction2sector(
    const double a, const unsigned int N)
{
    const int idx = round(0.5 * (N * (1 + mrpt::math::wrapToPi(a) / M_PI) - 1));
    if (idx < 0)
        return 0;
    else
        return static_cast<unsigned int>(idx);
}

uint8_t CLogFileRecord_ND::serializeGetVersion() const { return 1; }
void CLogFileRecord_ND::serializeTo(mrpt::serialization::CArchive& out) const
{
    out << gaps_ini << gaps_end << gaps_eval;
    out << selectedSector << evaluation << riskEvaluation
        << (uint32_t)situation;
}

void CLogFileRecord_ND::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        {
            int32_t n;

            in >> n;
            gaps_ini.resize(n);
            gaps_end.resize(n);
            in.ReadBuffer(&(*gaps_ini.begin()), sizeof(gaps_ini[0]) * n);
            in.ReadBuffer(&(*gaps_end.begin()), sizeof(gaps_end[0]) * n);

            in >> n;
            gaps_eval.resize(n);
            in.ReadBuffer(&(*gaps_eval.begin()), sizeof(gaps_eval[0]) * n);

            in >> selectedSector >> evaluation >> riskEvaluation >> n;

            situation = (CHolonomicND::TSituations)n;
        }
        break;
        case 1:
        {
            uint32_t n;
            in >> gaps_ini >> gaps_end >> gaps_eval;
            in >> selectedSector >> evaluation >> riskEvaluation >> n;
            situation = (CHolonomicND::TSituations)n;
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
    };
}

/*---------------------------------------------------------------
                        TOptions
  ---------------------------------------------------------------*/
CHolonomicND::TOptions::TOptions()
    :  // Default values:
      TOO_CLOSE_OBSTACLE(0.15),
      WIDE_GAP_SIZE_PERCENT(0.25),
      RISK_EVALUATION_SECTORS_PERCENT(0.10),
      RISK_EVALUATION_DISTANCE(0.4),
      MAX_SECTOR_DIST_FOR_D2_PERCENT(0.25),
      TARGET_SLOW_APPROACHING_DISTANCE(0.60),
      factorWeights{1.0, 0.5, 2.0, 0.4}
{
}

void CHolonomicND::TOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
    MRPT_START

    // Load from config text:
    MRPT_LOAD_CONFIG_VAR(WIDE_GAP_SIZE_PERCENT, double, source, section);
    MRPT_LOAD_CONFIG_VAR(
        MAX_SECTOR_DIST_FOR_D2_PERCENT, double, source, section);
    MRPT_LOAD_CONFIG_VAR(
        RISK_EVALUATION_SECTORS_PERCENT, double, source, section);
    MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_DISTANCE, double, source, section);
    MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, source, section);
    MRPT_LOAD_CONFIG_VAR(
        TARGET_SLOW_APPROACHING_DISTANCE, double, source, section);

    source.read_vector(
        section, "factorWeights", std::vector<double>(), factorWeights, true);
    ASSERT_(factorWeights.size() == 4);

    MRPT_END
}

void CHolonomicND::TOptions::saveToConfigFile(
    mrpt::config::CConfigFileBase& c, const std::string& s) const
{
    MRPT_START;
    const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
              WV = mrpt::config::MRPT_SAVE_VALUE_PADDING();

    MRPT_SAVE_CONFIG_VAR_COMMENT(WIDE_GAP_SIZE_PERCENT, "");
    MRPT_SAVE_CONFIG_VAR_COMMENT(MAX_SECTOR_DIST_FOR_D2_PERCENT, "");
    MRPT_SAVE_CONFIG_VAR_COMMENT(RISK_EVALUATION_SECTORS_PERCENT, "");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        RISK_EVALUATION_DISTANCE, "In normalized ps-meters [0,1]");
    MRPT_SAVE_CONFIG_VAR_COMMENT(TOO_CLOSE_OBSTACLE, "For stopping gradually");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        TARGET_SLOW_APPROACHING_DISTANCE, "In normalized ps-meters");

    ASSERT_EQUAL_(factorWeights.size(), 4);
    c.write(
        s, "factorWeights",
        mrpt::format(
            "%.2f %.2f %.2f %.2f", factorWeights[0], factorWeights[1],
            factorWeights[2], factorWeights[3]),
        WN, WV,
        "[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
        "(Euclidean), [3]=Hysteresis");

    MRPT_END
}

uint8_t CHolonomicND::serializeGetVersion() const { return 0; }
void CHolonomicND::serializeTo(mrpt::serialization::CArchive& out) const
{
    // Params:
    out << options.factorWeights << options.MAX_SECTOR_DIST_FOR_D2_PERCENT
        << options.RISK_EVALUATION_DISTANCE
        << options.RISK_EVALUATION_SECTORS_PERCENT
        << options.TARGET_SLOW_APPROACHING_DISTANCE
        << options.TOO_CLOSE_OBSTACLE << options.WIDE_GAP_SIZE_PERCENT;
    // State:
    out << m_last_selected_sector;
}
void CHolonomicND::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        {
            // Params:
            in >> options.factorWeights >>
                options.MAX_SECTOR_DIST_FOR_D2_PERCENT >>
                options.RISK_EVALUATION_DISTANCE >>
                options.RISK_EVALUATION_SECTORS_PERCENT >>
                options.TARGET_SLOW_APPROACHING_DISTANCE >>
                options.TOO_CLOSE_OBSTACLE >> options.WIDE_GAP_SIZE_PERCENT;
            // State:
            in >> m_last_selected_sector;
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
    };
}