CPTG_DiffDrive_CollisionGridBased.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/tpspace/CPTG_DiffDrive_CollisionGridBased.h>

#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CFileGZOutputStream.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/math/geometry.h>
#include <mrpt/utils/stl_serialization.h>
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>

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()
        : V_MAX(.0),
          W_MAX(.0),
          turningRadiusReference(.10),
          m_resolution(0.05),
          m_stepTimeDuration(0.01),
          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::utils::DEG2RAD(120);
}

void CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile(
        const mrpt::utils::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::utils::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::utils::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::utils::CStream& mrpt::nav::operator<<(
        mrpt::utils::CStream& o, const mrpt::nav::TCPoint& p)
{
        o << p.x << p.y << p.phi << p.t << p.dist << p.v << p.w;
        return o;
}
mrpt::utils::CStream& mrpt::nav::operator>>(
        mrpt::utils::CStream& 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::math::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::utils::keep_max(max_acc_lin, acc_lin);
                                        mrpt::utils::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 uint32_t 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::utils::keep_min(cell->k_min, k);
                                mrpt::utils::keep_max(cell->k_max, k);
                                mrpt::utils::keep_min(cell->n_min, n);
                                mrpt::utils::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);

        mrpt::kinematics::CVehicleVelCmd_DiffDriven* 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:
        TCollisionCell::iterator itK = cell->end();
        for (TCollisionCell::iterator 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::utils::CFileGZOutputStream fo(filename);
                if (!fo.fileOpenCorrectly()) return false;

                const uint32_t n = 1;  // for backwards compatibility...
                fo << n;
                return m_collisionGrid.saveToFile(&fo, 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::utils::CFileGZInputStream fi(filename);
                if (!fi.fileOpenCorrectly()) return false;

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

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

const uint32_t COLGRID_FILE_MAGIC = 0xC0C0C0C3;

/*---------------------------------------------------------------
                                        Save to file
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::saveToFile(
        mrpt::utils::CStream* 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::utils::CStream* 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 (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::math::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::utils::keep_min(k_min, cell->k_min);
                                        mrpt::utils::keep_max(k_max, cell->k_max);

                                        mrpt::utils::keep_min(n_min, cell->n_min);
                                        mrpt::utils::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::utils::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::utils::keep_max(bb_max.x, transf_shape[m].x);
                                        mrpt::utils::keep_max(bb_max.y, transf_shape[m].y);
                                        mrpt::utils::keep_min(bb_min.x, transf_shape[m].x);
                                        mrpt::utils::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 (TCollisionCell::const_iterator i = cell.begin(); i != cell.end(); ++i)
        {
                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 (TCollisionCell::const_iterator i = cell.begin(); i != cell.end(); ++i)
                if (i->first == k)
                {
                        const double dist = i->second;
                        internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
                }
}

void CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(
        mrpt::utils::CStream& 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::utils::CStream& 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;
}