CPTG_DiffDrive_CollisionGridBased.cpp

Go to the documentation of this file.

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

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

#include <mrpt/nav/tpspace/CPTG_DiffDrive_CollisionGridBased.h>

#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>
#include <mrpt/math/geometry.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/CTicTac.h>
#include <iostream>

using namespace mrpt::nav;

/** Constructor: possible values in "params":
 *   - ref_distance: The maximum distance in PTGs
 *   - resolution: The cell size
 *   - v_max, w_max: Maximum robot speeds.
 */
CPTG_DiffDrive_CollisionGridBased::CPTG_DiffDrive_CollisionGridBased()
    : m_collisionGrid(-1, 1, -1, 1, 0.5, this)
{
}

void CPTG_DiffDrive_CollisionGridBased::loadDefaultParams()
{
    CParameterizedTrajectoryGenerator::loadDefaultParams();
    CPTG_RobotShape_Polygonal::loadDefaultParams();

    m_resolution = 0.10;
    V_MAX = 1.0;
    W_MAX = mrpt::DEG2RAD(120);
}

void CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
{
    CParameterizedTrajectoryGenerator::loadFromConfigFile(cfg, sSection);
    CPTG_RobotShape_Polygonal::loadShapeFromConfigFile(cfg, sSection);

    MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
        resolution, double, m_resolution, cfg, sSection);
    MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
        v_max_mps, double, V_MAX, cfg, sSection);
    MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(
        w_max_dps, double, W_MAX, cfg, sSection);
    MRPT_LOAD_CONFIG_VAR(turningRadiusReference, double, cfg, sSection);
}
void CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(
    mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
{
    MRPT_START
    const int WN = 25, WV = 30;

    CParameterizedTrajectoryGenerator::saveToConfigFile(cfg, sSection);

    cfg.write(
        sSection, "resolution", m_resolution, WN, WV,
        "Resolution of the collision-check look-up-table [m].");
    cfg.write(
        sSection, "v_max_mps", V_MAX, WN, WV,
        "Maximum linear velocity for trajectories [m/s].");
    cfg.write(
        sSection, "w_max_dps", mrpt::RAD2DEG(W_MAX), WN, WV,
        "Maximum angular velocity for trajectories [deg/s].");
    cfg.write(
        sSection, "turningRadiusReference", turningRadiusReference, WN, WV,
        "An approximate dimension of the robot (not a critical parameter) "
        "[m].");

    CPTG_RobotShape_Polygonal::saveToConfigFile(cfg, sSection);

    MRPT_END
}

mrpt::serialization::CArchive& mrpt::nav::operator<<(
    mrpt::serialization::CArchive& o, const mrpt::nav::TCPoint& p)
{
    o << p.x << p.y << p.phi << p.t << p.dist << p.v << p.w;
    return o;
}
mrpt::serialization::CArchive& mrpt::nav::operator>>(
    mrpt::serialization::CArchive& i, mrpt::nav::TCPoint& p)
{
    i >> p.x >> p.y >> p.phi >> p.t >> p.dist >> p.v >> p.w;
    return i;
}

/*---------------------------------------------------------------
                    simulateTrajectories
    Solve trajectories and fill cells.
  ---------------------------------------------------------------*/
void CPTG_DiffDrive_CollisionGridBased::simulateTrajectories(
    float max_time, float max_dist, unsigned int max_n, float diferencial_t,
    float min_dist, float* out_max_acc_v, float* out_max_acc_w)
{
    using mrpt::square;

    internal_deinitialize();  // Free previous paths

    m_stepTimeDuration = diferencial_t;

    // Reserve the size in the buffers:
    m_trajectory.resize(m_alphaValuesCount);

    const float radio_max_robot = 1.0f;  // Arbitrary "robot radius", only to
    // determine the spacing of points
    // under pure rotation

    // Aux buffer:
    TCPointVector points;

    float ult_dist, ult_dist1, ult_dist2;

    // For the grid:
    float x_min = 1e3f, x_max = -1e3;
    float y_min = 1e3f, y_max = -1e3;

    // Para averiguar las maximas ACELERACIONES lineales y angulares:
    float max_acc_lin, max_acc_ang;

    max_acc_lin = max_acc_ang = 0;

    try
    {
        for (unsigned int k = 0; k < m_alphaValuesCount; k++)
        {
            // Simulate / evaluate the trajectory selected by this "alpha":
            // ------------------------------------------------------------
            const float alpha = index2alpha(k);

            points.clear();
            float t = .0f, dist = .0f, girado = .0f;
            float x = .0f, y = .0f, phi = .0f, v = .0f, w = .0f, _x = .0f,
                  _y = .0f, _phi = .0f;

            // Sliding window with latest movement commands (for the optional
            // low-pass filtering):
            float last_vs[2] = {.0f, .0f}, last_ws[2] = {.0f, .0f};

            // Add the first, initial point:
            points.push_back(TCPoint(x, y, phi, t, dist, v, w));

            // Simulate until...
            while (t < max_time && dist < max_dist && points.size() < max_n &&
                   fabs(girado) < 1.95 * M_PI)
            {
                // Max. aceleraciones:
                if (t > 1)
                {
                    float acc_lin =
                        fabs((last_vs[0] - last_vs[1]) / diferencial_t);
                    float acc_ang =
                        fabs((last_ws[0] - last_ws[1]) / diferencial_t);
                    mrpt::keep_max(max_acc_lin, acc_lin);
                    mrpt::keep_max(max_acc_ang, acc_ang);
                }

                // Compute new movement command (v,w):
                ptgDiffDriveSteeringFunction(alpha, t, x, y, phi, v, w);

                // History of v/w ----------------------------------
                last_vs[1] = last_vs[0];
                last_ws[1] = last_ws[0];
                last_vs[0] = v;
                last_ws[0] = w;
                // -------------------------------------------

                // Finite difference equation:
                x += cos(phi) * v * diferencial_t;
                y += sin(phi) * v * diferencial_t;
                phi += w * diferencial_t;

                // Counters:
                girado += w * diferencial_t;

                float v_inTPSpace =
                    sqrt(square(v) + square(w * turningRadiusReference));

                dist += v_inTPSpace * diferencial_t;

                t += diferencial_t;

                // Save sample if we moved far enough:
                ult_dist1 = sqrt(square(_x - x) + square(_y - y));
                ult_dist2 = fabs(radio_max_robot * (_phi - phi));
                ult_dist = std::max(ult_dist1, ult_dist2);

                if (ult_dist > min_dist)
                {
                    // Set the (v,w) to the last record:
                    points.back().v = v;
                    points.back().w = w;

                    // And add the new record:
                    points.push_back(TCPoint(x, y, phi, t, dist, v, w));

                    // For the next iter:
                    _x = x;
                    _y = y;
                    _phi = phi;
                }

                // for the grid:
                x_min = std::min(x_min, x);
                x_max = std::max(x_max, x);
                y_min = std::min(y_min, y);
                y_max = std::max(y_max, y);
            }

            // Add the final point:
            points.back().v = v;
            points.back().w = w;
            points.push_back(TCPoint(x, y, phi, t, dist, v, w));

            // Save data to C-Space path structure:
            m_trajectory[k] = points;

        }  // end for "k"

        // Save accelerations
        if (out_max_acc_v) *out_max_acc_v = max_acc_lin;
        if (out_max_acc_w) *out_max_acc_w = max_acc_ang;

        // --------------------------------------------------------
        // Build the speeding-up grid for lambda function:
        // --------------------------------------------------------
        const TCellForLambdaFunction defaultCell;
        m_lambdaFunctionOptimizer.setSize(
            x_min - 0.5f, x_max + 0.5f, y_min - 0.5f, y_max + 0.5f, 0.25f,
            &defaultCell);

        for (uint16_t k = 0; k < m_alphaValuesCount; k++)
        {
            const auto M = static_cast<uint32_t>(m_trajectory[k].size());
            for (uint32_t n = 0; n < M; n++)
            {
                TCellForLambdaFunction* cell =
                    m_lambdaFunctionOptimizer.cellByPos(
                        m_trajectory[k][n].x, m_trajectory[k][n].y);
                ASSERT_(cell);
                // Keep limits:
                mrpt::keep_min(cell->k_min, k);
                mrpt::keep_max(cell->k_max, k);
                mrpt::keep_min(cell->n_min, n);
                mrpt::keep_max(cell->n_max, n);
            }
        }
    }
    catch (...)
    {
        std::cout << format(
            "[CPTG_DiffDrive_CollisionGridBased::simulateTrajectories] "
            "Simulation aborted: unexpected exception!\n");
    }
}

/** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s),  [1]:
 * angular velocity (rad/s) */
mrpt::kinematics::CVehicleVelCmd::Ptr
    CPTG_DiffDrive_CollisionGridBased::directionToMotionCommand(
        uint16_t k) const
{
    float v, w;
    ptgDiffDriveSteeringFunction(index2alpha(k), 0, 0, 0, 0, v, w);

    auto* cmd = new mrpt::kinematics::CVehicleVelCmd_DiffDriven();
    cmd->lin_vel = v;
    cmd->ang_vel = w;
    return mrpt::kinematics::CVehicleVelCmd::Ptr(cmd);
}

/*---------------------------------------------------------------
                    getTPObstacle
  ---------------------------------------------------------------*/
const CPTG_DiffDrive_CollisionGridBased::TCollisionCell&
    CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::getTPObstacle(
        const float obsX, const float obsY) const
{
    static const TCollisionCell emptyCell;
    const TCollisionCell* cell = cellByPos(obsX, obsY);
    return cell != nullptr ? *cell : emptyCell;
}

/*---------------------------------------------------------------
    Updates the info into a cell: It updates the cell only
      if the distance d for the path k is lower than the previous value:
  ---------------------------------------------------------------*/
void CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::updateCellInfo(
    const unsigned int icx, const unsigned int icy, const uint16_t k,
    const float dist)
{
    TCollisionCell* cell = cellByIndex(icx, icy);
    if (!cell) return;

    // For such a small number of elements, brute-force search is not such a bad
    // idea:
    auto itK = cell->end();
    for (auto it = cell->begin(); it != cell->end(); ++it)
        if (it->first == k)
        {
            itK = it;
            break;
        }

    if (itK == cell->end())
    {  // New entry:
        cell->push_back(std::make_pair(k, dist));
    }
    else
    {  // Only update that "k" if the distance is shorter now:
        if (dist < itK->second) itK->second = dist;
    }
}

/*---------------------------------------------------------------
                    Save to file
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::saveColGridsToFile(
    const std::string& filename,
    const mrpt::math::CPolygon& computed_robotShape) const
{
    try
    {
        mrpt::io::CFileGZOutputStream fo(filename);
        if (!fo.fileOpenCorrectly()) return false;

        const uint32_t n = 1;  // for backwards compatibility...
        auto arch = mrpt::serialization::archiveFrom(fo);
        arch << n;
        return m_collisionGrid.saveToFile(&arch, computed_robotShape);
    }
    catch (...)
    {
        return false;
    }
}

/*---------------------------------------------------------------
                    Load from file
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::loadColGridsFromFile(
    const std::string& filename, const mrpt::math::CPolygon& current_robotShape)
{
    try
    {
        mrpt::io::CFileGZInputStream fi(filename);
        if (!fi.fileOpenCorrectly()) return false;
        auto arch = mrpt::serialization::archiveFrom(fi);

        uint32_t n;
        arch >> n;
        if (n != 1)
            return false;  // Incompatible (old) format, just discard and
        // recompute.

        return m_collisionGrid.loadFromFile(&arch, current_robotShape);
    }
    catch (...)
    {
        return false;
    }
}

const uint32_t COLGRID_FILE_MAGIC = 0xC0C0C0C3;

/*---------------------------------------------------------------
                    Save to file
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::saveToFile(
    mrpt::serialization::CArchive* f,
    const mrpt::math::CPolygon& computed_robotShape) const
{
    try
    {
        if (!f) return false;

        const uint8_t serialize_version =
            2;  // v1: As of jun 2012, v2: As of dec-2013

        // Save magic signature && serialization version:
        *f << COLGRID_FILE_MAGIC << serialize_version;

        // Robot shape:
        *f << computed_robotShape;

        // and standard PTG data:
        *f << m_parent->getDescription() << m_parent->getAlphaValuesCount()
           << static_cast<float>(m_parent->getMax_V())
           << static_cast<float>(m_parent->getMax_W());

        *f << m_x_min << m_x_max << m_y_min << m_y_max;
        *f << m_resolution;

        // v1 was:  *f << m_map;
        uint32_t N = m_map.size();
        *f << N;
        for (uint32_t i = 0; i < N; i++)
        {
            uint32_t M = m_map[i].size();
            *f << M;
            for (uint32_t k = 0; k < M; k++)
                *f << m_map[i][k].first << m_map[i][k].second;
        }

        return true;
    }
    catch (...)
    {
        return false;
    }
}

/*---------------------------------------------------------------
                        loadFromFile
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::loadFromFile(
    mrpt::serialization::CArchive* f,
    const mrpt::math::CPolygon& current_robotShape)
{
    try
    {
        if (!f) return false;

        // Return false if the file contents doesn't match what we expected:
        uint32_t file_magic;
        *f >> file_magic;

        // It doesn't seem to be a valid file or was in an old format, just
        // recompute the grid:
        if (COLGRID_FILE_MAGIC != file_magic) return false;

        uint8_t serialized_version;
        *f >> serialized_version;

        switch (serialized_version)
        {
            case 2:
            {
                mrpt::math::CPolygon stored_shape;
                *f >> stored_shape;

                const bool shapes_match =
                    (stored_shape.size() == current_robotShape.size() &&
                     std::equal(
                         stored_shape.begin(), stored_shape.end(),
                         current_robotShape.begin()));

                if (!shapes_match)
                    return false;  // Must recompute if the robot shape changed.
            }
            break;

            case 1:
            default:
                // Unknown version: Maybe we are loading a file from a more
                // recent version of MRPT? Whatever, we can't read it: It's
                // safer just to re-generate the PTG data
                return false;
        };

        // Standard PTG data:
        const std::string expected_desc = m_parent->getDescription();
        std::string desc;
        *f >> desc;
        if (desc != expected_desc) return false;

// and standard PTG data:
#define READ_UINT16_CHECK_IT_MATCHES_STORED(_VAR) \
    {                                             \
        uint16_t ff;                              \
        *f >> ff;                                 \
        if (ff != _VAR) return false;             \
    }
#define READ_FLOAT_CHECK_IT_MATCHES_STORED(_VAR)       \
    {                                                  \
        float ff;                                      \
        *f >> ff;                                      \
        if (std::abs(ff - _VAR) > 1e-4f) return false; \
    }
#define READ_DOUBLE_CHECK_IT_MATCHES_STORED(_VAR)     \
    {                                                 \
        double ff;                                    \
        *f >> ff;                                     \
        if (std::abs(ff - _VAR) > 1e-6) return false; \
    }

        READ_UINT16_CHECK_IT_MATCHES_STORED(m_parent->getAlphaValuesCount())
        READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_V())
        READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_W())

        // Cell dimensions:
        READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_x_min)
        READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_x_max)
        READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_y_min)
        READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_y_max)
        READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_resolution)

        // OK, all parameters seem to be exactly the same than when we
        // precomputed the table: load it.
        // v1 was:  *f >> m_map;
        uint32_t N;
        *f >> N;
        m_map.resize(N);
        for (uint32_t i = 0; i < N; i++)
        {
            uint32_t M;
            *f >> M;
            m_map[i].resize(M);
            for (uint32_t k = 0; k < M; k++)
                *f >> m_map[i][k].first >> m_map[i][k].second;
        }

        return true;
    }
    catch (const std::exception& e)
    {
        std::cerr << "[CCollisionGrid::loadFromFile] " << e.what();
        return false;
    }
    catch (...)
    {
        return false;
    }
}

bool CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(
    double x, double y, int& out_k, double& out_d, double tolerance_dist) const
{
    using mrpt::square;

    ASSERTMSG_(
        m_alphaValuesCount > 0,
        "Have you called simulateTrajectories() first?");

    // -------------------------------------------------------------------
    // Optimization: (24-JAN-2007 @ Jose Luis Blanco):
    //  Use a "grid" to determine the range of [k,d] values to check!!
    //  If the point (x,y) is not found in the grid, then directly skip
    //  to the next step.
    // -------------------------------------------------------------------
    uint16_t k_min = 0, k_max = m_alphaValuesCount - 1;
    uint32_t n_min = 0, n_max = 0;
    bool at_least_one = false;

    // Cell indexes:
    int cx0 = m_lambdaFunctionOptimizer.x2idx(x);
    int cy0 = m_lambdaFunctionOptimizer.y2idx(y);

    // (cx,cy)
    for (int cx = cx0 - 1; cx <= cx0 + 1; cx++)
    {
        for (int cy = cy0 - 1; cy <= cy0 + 1; cy++)
        {
            const TCellForLambdaFunction* cell =
                m_lambdaFunctionOptimizer.cellByIndex(cx, cy);
            if (cell && !cell->isEmpty())
            {
                if (!at_least_one)
                {
                    k_min = cell->k_min;
                    k_max = cell->k_max;
                    n_min = cell->n_min;
                    n_max = cell->n_max;
                    at_least_one = true;
                }
                else
                {
                    mrpt::keep_min(k_min, cell->k_min);
                    mrpt::keep_max(k_max, cell->k_max);

                    mrpt::keep_min(n_min, cell->n_min);
                    mrpt::keep_max(n_max, cell->n_max);
                }
            }
        }
    }

    // Try to find a closest point to the paths:
    // ----------------------------------------------
    int selected_k = -1;
    float selected_d = 0;
    float selected_dist = std::numeric_limits<float>::max();

    if (at_least_one)  // Otherwise, don't even lose time checking...
    {
        ASSERT_BELOW_(k_max, m_trajectory.size());
        for (int k = k_min; k <= k_max; k++)
        {
            const size_t n_real = m_trajectory[k].size();
            const uint32_t n_max_this =
                std::min(static_cast<uint32_t>(n_real ? n_real - 1 : 0), n_max);

            for (uint32_t n = n_min; n <= n_max_this; n++)
            {
                const float dist_a_punto = square(m_trajectory[k][n].x - x) +
                                           square(m_trajectory[k][n].y - y);
                if (dist_a_punto < selected_dist)
                {
                    selected_dist = dist_a_punto;
                    selected_k = k;
                    selected_d = m_trajectory[k][n].dist;
                }
            }
        }
    }

    if (selected_k != -1)
    {
        out_k = selected_k;
        out_d = selected_d / refDistance;
        return (selected_dist <= square(tolerance_dist));
    }

    // If not found, compute an extrapolation:

    // ------------------------------------------------------------------------------------
    // Given a point (x,y), compute the "k_closest" whose extrapolation
    //  is closest to the point, and the associated "d_closest" distance,
    //  which can be normalized by "1/refDistance" to get TP-Space distances.
    // ------------------------------------------------------------------------------------
    selected_dist = std::numeric_limits<float>::max();
    for (uint16_t k = 0; k < m_alphaValuesCount; k++)
    {
        const int n = int(m_trajectory[k].size()) - 1;
        const float dist_a_punto = square(m_trajectory[k][n].dist) +
                                   square(m_trajectory[k][n].x - x) +
                                   square(m_trajectory[k][n].y - y);

        if (dist_a_punto < selected_dist)
        {
            selected_dist = dist_a_punto;
            selected_k = k;
            selected_d = dist_a_punto;
        }
    }

    selected_d = std::sqrt(selected_d);

    out_k = selected_k;
    out_d = selected_d / refDistance;

    // If the target dist. > refDistance, then it's normal that we had to
    // extrapolate.
    // Otherwise, it may actually mean that the target is not reachable by this
    // set of paths:
    const float target_dist = std::sqrt(x * x + y * y);
    return (target_dist > target_dist);
}

void CPTG_DiffDrive_CollisionGridBased::setRefDistance(const double refDist)
{
    ASSERTMSG_(
        m_trajectory.empty(),
        "Changing reference distance not allowed in this class after "
        "initialization!");
    this->refDistance = refDist;
}

void CPTG_DiffDrive_CollisionGridBased::internal_processNewRobotShape()
{
    ASSERTMSG_(
        m_trajectory.empty(),
        "Changing robot shape not allowed in this class after initialization!");
}

void CPTG_DiffDrive_CollisionGridBased::internal_deinitialize()
{
    m_trajectory.clear();  // Free trajectories
}

void CPTG_DiffDrive_CollisionGridBased::internal_initialize(
    const std::string& cacheFilename, const bool verbose)
{
    using namespace std;

    MRPT_START

    if (verbose)
        cout << endl
             << "[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... "
                "*** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **"
             << endl;

    // Sanity checks:
    ASSERTMSG_(!m_robotShape.empty(), "Robot shape was not defined");
    ASSERTMSG_(
        m_robotShape.size() >= 3, "Robot shape must have 3 or more vertices");
    ASSERT_(refDistance > 0);
    ASSERT_(V_MAX > 0);
    ASSERT_(W_MAX > 0);
    ASSERT_(m_resolution > 0);

    mrpt::system::CTicTac tictac;
    tictac.Tic();

    if (verbose) cout << "Initializing PTG '" << cacheFilename << "'...";

    // Simulate paths:
    const float min_dist = 0.015f;
    simulateTrajectories(
        100,  // max.tim,
        refDistance,  // max.dist,
        10 * refDistance / min_dist,  // max.n,
        0.0005f,  // diferencial_t
        min_dist  // min_dist
    );

    // Just for debugging, etc.
    // debugDumpInFiles(n);

    // Check for collisions between the robot shape and the grid cells:
    // ----------------------------------------------------------------------------
    m_collisionGrid.setSize(
        -refDistance, refDistance, -refDistance, refDistance, m_resolution);

    const size_t Ki = getAlphaValuesCount();
    ASSERTMSG_(Ki > 0, "The PTG seems to be not initialized!");

    // Load the cached version, if possible
    if (loadColGridsFromFile(cacheFilename, m_robotShape))
    {
        if (verbose) cout << "loaded from file OK" << endl;
    }
    else
    {
        // BUGFIX: In case we start reading the file and in the end detected an
        // error,
        //         we must make sure that there's space enough for the grid:
        m_collisionGrid.setSize(
            -refDistance, refDistance, -refDistance, refDistance,
            m_collisionGrid.getResolution());

        const int grid_cx_max = m_collisionGrid.getSizeX() - 1;
        const int grid_cy_max = m_collisionGrid.getSizeY() - 1;
        const double half_cell = m_collisionGrid.getResolution() * 0.5;

        const size_t nVerts = m_robotShape.verticesCount();
        std::vector<mrpt::math::TPoint2D> transf_shape(
            nVerts);  // The robot shape at each location

        // RECOMPUTE THE COLLISION GRIDS:
        // ---------------------------------------
        for (size_t k = 0; k < Ki; k++)
        {
            const size_t nPoints = getPathStepCount(k);
            ASSERT_(nPoints > 1);
            for (size_t n = 0; n < (nPoints - 1); n++)
            {
                // Translate and rotate the robot shape at this C-Space pose:
                mrpt::math::TPose2D p;
                getPathPose(k, n, p);

                mrpt::math::TPoint2D bb_min(
                    std::numeric_limits<double>::max(),
                    std::numeric_limits<double>::max());
                mrpt::math::TPoint2D bb_max(
                    -std::numeric_limits<double>::max(),
                    -std::numeric_limits<double>::max());

                for (size_t m = 0; m < nVerts; m++)
                {
                    transf_shape[m].x =
                        p.x + cos(p.phi) * m_robotShape.GetVertex_x(m) -
                        sin(p.phi) * m_robotShape.GetVertex_y(m);
                    transf_shape[m].y =
                        p.y + sin(p.phi) * m_robotShape.GetVertex_x(m) +
                        cos(p.phi) * m_robotShape.GetVertex_y(m);
                    mrpt::keep_max(bb_max.x, transf_shape[m].x);
                    mrpt::keep_max(bb_max.y, transf_shape[m].y);
                    mrpt::keep_min(bb_min.x, transf_shape[m].x);
                    mrpt::keep_min(bb_min.y, transf_shape[m].y);
                }

                // Robot shape polygon:
                const mrpt::math::TPolygon2D poly(transf_shape);

                // Get the range of cells that may collide with this shape:
                const int ix_min =
                    std::max(0, m_collisionGrid.x2idx(bb_min.x) - 1);
                const int iy_min =
                    std::max(0, m_collisionGrid.y2idx(bb_min.y) - 1);
                const int ix_max =
                    std::min(m_collisionGrid.x2idx(bb_max.x) + 1, grid_cx_max);
                const int iy_max =
                    std::min(m_collisionGrid.y2idx(bb_max.y) + 1, grid_cy_max);

                for (int ix = ix_min; ix < ix_max; ix++)
                {
                    const double cx = m_collisionGrid.idx2x(ix) - half_cell;

                    for (int iy = iy_min; iy < iy_max; iy++)
                    {
                        const double cy = m_collisionGrid.idx2y(iy) - half_cell;

                        if (poly.contains(mrpt::math::TPoint2D(cx, cy)))
                        {
                            // Collision!! Update cell info:
                            const float d = this->getPathDist(k, n);
                            m_collisionGrid.updateCellInfo(ix, iy, k, d);
                            m_collisionGrid.updateCellInfo(ix - 1, iy, k, d);
                            m_collisionGrid.updateCellInfo(ix, iy - 1, k, d);
                            m_collisionGrid.updateCellInfo(
                                ix - 1, iy - 1, k, d);
                        }
                    }  // for iy
                }  // for ix

            }  // n

            if (verbose) cout << k << "/" << Ki << ",";
        }  // k

        if (verbose) cout << format("Done! [%.03f sec]\n", tictac.Tac());

        // save it to the cache file for the next run:
        saveColGridsToFile(cacheFilename, m_robotShape);

    }  // "else" recompute all PTG

    MRPT_END
}

size_t CPTG_DiffDrive_CollisionGridBased::getPathStepCount(uint16_t k) const
{
    ASSERT_(k < m_trajectory.size());

    return m_trajectory[k].size();
}

void CPTG_DiffDrive_CollisionGridBased::getPathPose(
    uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const
{
    ASSERT_(k < m_trajectory.size());
    ASSERT_(step < m_trajectory[k].size());

    p.x = m_trajectory[k][step].x;
    p.y = m_trajectory[k][step].y;
    p.phi = m_trajectory[k][step].phi;
}

double CPTG_DiffDrive_CollisionGridBased::getPathDist(
    uint16_t k, uint32_t step) const
{
    ASSERT_(k < m_trajectory.size());
    ASSERT_(step < m_trajectory[k].size());

    return m_trajectory[k][step].dist;
}

bool CPTG_DiffDrive_CollisionGridBased::getPathStepForDist(
    uint16_t k, double dist, uint32_t& out_step) const
{
    ASSERT_(k < m_trajectory.size());
    const size_t numPoints = m_trajectory[k].size();

    ASSERT_(numPoints > 0);

    for (size_t n = 0; n < numPoints - 1; n++)
    {
        if (m_trajectory[k][n + 1].dist >= dist)
        {
            out_step = n;
            return true;
        }
    }

    out_step = numPoints - 1;
    return false;
}

void CPTG_DiffDrive_CollisionGridBased::updateTPObstacle(
    double ox, double oy, std::vector<double>& tp_obstacles) const
{
    ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
    const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
    // Keep the minimum distance:
    for (const auto& i : cell)
    {
        const double dist = i.second;
        internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacles[i.first]);
    }
}

void CPTG_DiffDrive_CollisionGridBased::updateTPObstacleSingle(
    double ox, double oy, uint16_t k, double& tp_obstacle_k) const
{
    ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
    const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
    // Keep the minimum distance:
    for (const auto& i : cell)
        if (i.first == k)
        {
            const double dist = i.second;
            internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
        }
}

void CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(
    mrpt::serialization::CArchive& in)
{
    CParameterizedTrajectoryGenerator::internal_readFromStream(in);
    CPTG_RobotShape_Polygonal::internal_shape_loadFromStream(in);

    uint8_t version;
    in >> version;
    switch (version)
    {
        case 0:
            internal_deinitialize();
            in >> V_MAX >> W_MAX >> turningRadiusReference >> m_robotShape >>
                m_resolution >> m_trajectory;
            break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}

void CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(
    mrpt::serialization::CArchive& out) const
{
    CParameterizedTrajectoryGenerator::internal_writeToStream(out);
    CPTG_RobotShape_Polygonal::internal_shape_saveToStream(out);

    const uint8_t version = 0;
    out << version;

    out << V_MAX << W_MAX << turningRadiusReference << m_robotShape
        << m_resolution << m_trajectory;
}

mrpt::kinematics::CVehicleVelCmd::Ptr
    CPTG_DiffDrive_CollisionGridBased::getSupportedKinematicVelocityCommand()
        const
{
    return mrpt::kinematics::CVehicleVelCmd::Ptr(
        new mrpt::kinematics::CVehicleVelCmd_DiffDriven());
}

double CPTG_DiffDrive_CollisionGridBased::getPathStepDuration() const
{
    return m_stepTimeDuration;
}