CHolonomicND.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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/utils/CStream.h>
#include <mrpt/utils/round.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>

using namespace mrpt;
using namespace mrpt::utils;
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::utils::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::utils::CConfigFileBase& INI_FILE)
{
        options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
}
void CHolonomicND::saveConfigFile(mrpt::utils::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::utils::keep_max(overall_max_dist, obstacles[i]);
                mrpt::utils::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::utils::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::utils::abs_diff(target_sector, gap.ini);
                unsigned int dist_end = mrpt::utils::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::utils::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::utils::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::utils::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);
}

/*---------------------------------------------------------------
                                        writeToStream
        Implements the writing to a CStream capability of
          CSerializable objects
  ---------------------------------------------------------------*/
void CLogFileRecord_ND::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 1;
        else
        {
                out << gaps_ini << gaps_end << gaps_eval;
                out << selectedSector << evaluation << riskEvaluation
                        << (uint32_t)situation;
        }
}

/*---------------------------------------------------------------
                                        readFromStream
  ---------------------------------------------------------------*/
void CLogFileRecord_ND::readFromStream(mrpt::utils::CStream& in, int 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::utils::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::utils::CConfigFileBase& c, const std::string& s) const
{
        MRPT_START;
        const int WN = mrpt::utils::MRPT_SAVE_NAME_PADDING(),
                          WV = mrpt::utils::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
}

void CHolonomicND::writeToStream(mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 0;
        else
        {
                // 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::readFromStream(mrpt::utils::CStream& in, int 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)
        };
}