DiffDriveCollisionGridBased.h

Go to the documentation of this file.

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

#include <mrpt/containers/CDynamicGrid.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/typemeta/TEnumType.h>

namespace mpp::ptg
{
struct TCPoint
{
    TCPoint() = default;
    TCPoint(
        const float x_, const float y_, const float phi_, const float t_,
        const float dist_, const float v_, const float w_)
        : x(x_), y(y_), phi(phi_), t(t_), dist(dist_), v(v_), w(w_)
    {
    }
    float x = 0, y = 0, phi = 0, t = 0, dist = 0, v = 0, w = 0;
};
using TCPointVector = std::vector<TCPoint>;

mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& o, const TCPoint& p);
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& i, TCPoint& p);

/** Base class for all PTGs suitable to non-holonomic, differentially-driven (or
 * Ackermann) vehicles
 * based on numerical integration of the trajectories and collision
 * look-up-table.
 * Regarding `initialize()`: in this this family of PTGs, the method builds the
 * collision grid or load it from a cache file.
 * Collision grids must be calculated before calling getTPObstacle(). Robot
 * shape must be set before initializing with setRobotShape().
 * The rest of PTG parameters should have been set at the constructor.
 *
 * \note This class is forked from mrpt::nav to expose more parameters related
 *       to the path simulation time steps.
 *
 */
class DiffDriveCollisionGridBased : public mrpt::nav::CPTG_RobotShape_Polygonal
{
   public:
    /** The main method to be implemented in derived classes: it defines the
     * differential-driven differential equation */
    virtual void ptgDiffDriveSteeringFunction(
        float alpha, float t, float x, float y, float phi, float& v,
        float& w) const = 0;

    /** @name Virtual interface of each PTG implementation
     *  @{ */
    // getDescription(): remains to be defined in derived classes.
    // setParams() to be defined in derived classses.

    /** The default implementation in this class relies on a look-up-table.
     * Derived classes may redefine this to closed-form expressions, when they
     * exist.
     * See full docs in base class
     * CParameterizedTrajectoryGenerator::inverseMap_WS2TP() */
    bool inverseMap_WS2TP(
        double x, double y, int& out_k, double& out_d,
        double tolerance_dist = 0.10) const override;

    /** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s),
     * [1]: angular velocity (rad/s).
     * See more docs in
     * CParameterizedTrajectoryGenerator::directionToMotionCommand() */
    mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(
        uint16_t k) const override;
    mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand()
        const override;

    /** Launches an exception in this class: it is not allowed in numerical
     * integration-based PTGs to change the reference distance
     * after initialization. */
    void setRefDistance(const double refDist) override;

    // Access to PTG paths (see docs in base class)
    size_t              getPathStepCount(uint16_t k) const override;
    mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const override;
    double              getPathDist(uint16_t k, uint32_t step) const override;
    bool                getPathStepForDist(
                       uint16_t k, double dist, uint32_t& out_step) const override;
    double getPathStepDuration() const override;
    double getMaxLinVel() const override { return V_MAX; }
    double getMaxAngVel() const override { return W_MAX; }
    void   updateTPObstacle(
          double ox, double oy, std::vector<double>& tp_obstacles) const override;
    void updateTPObstacleSingle(
        double ox, double oy, uint16_t k, double& tp_obstacle_k) const override;

    /** This family of PTGs ignores the dynamic states */
    void onNewNavDynamicState() override
    {
        // Do nothing.
    }

    /** @} */  // --- end of virtual methods

    double getMax_V() const { return V_MAX; }
    double getMax_W() const { return W_MAX; }

   protected:
    DiffDriveCollisionGridBased();

    void internal_processNewRobotShape() override;
    void internal_initialize(
        const std::string& cacheFilename = std::string(),
        const bool         verbose       = true) override;
    void internal_deinitialize() override;

    /** Possible values in "params" (those in CParameterizedTrajectoryGenerator,
     * which is called internally, plus):
     *   - `${sKeyPrefix}resolution`: The cell size
     *   - `${sKeyPrefix}v_max`, ``${sKeyPrefix}w_max`: Maximum robot speeds.
     *   - `${sKeyPrefix}shape_x{0,1,2..}`, ``${sKeyPrefix}shape_y{0,1,2..}`:
     * Polygonal robot shape [Optional, can be also set via
     * `setRobotPolygonShape()`]
     *
     * See docs of derived classes for additional parameters in setParams()
     */
    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& cfg,
        const std::string&                   sSection) override;
    void saveToConfigFile(
        mrpt::config::CConfigFileBase& cfg,
        const std::string&             sSection) const override;

    void loadDefaultParams() override;

    double                     V_MAX{.0}, W_MAX{.0};
    double                     turningRadiusReference{.10};
    std::vector<TCPointVector> m_trajectory;
    double                     m_resolution{0.05};
    double                     m_stepTimeDuration{0.01};

    void internal_readFromStream(mrpt::serialization::CArchive& in) override;
    void internal_writeToStream(
        mrpt::serialization::CArchive& out) const override;

    /** Numerically solve the diferential equations to generate a family of
     * trajectories */
    void simulateTrajectories(float max_time, float max_dist, float dt);

    mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const override;

    /**  A list of all the pairs (alpha,distance) such as the robot collides at
     *that cell.
     *  - map key   (uint16_t) -> alpha value (k)
     *   - map value (float)    -> the MINIMUM distance (d), in meters,
     *associated with that "k".
     */
    using TCollisionCell = std::vector<std::pair<uint16_t, float>>;

    /** An internal class for storing the collision grid  */
    class CCollisionGrid : public mrpt::containers::CDynamicGrid<TCollisionCell>
    {
       private:
        DiffDriveCollisionGridBased const* m_parent;

       public:
        CCollisionGrid(
            float x_min, float x_max, float y_min, float y_max,
            float resolution, DiffDriveCollisionGridBased* parent)
            : mrpt::containers::CDynamicGrid<TCollisionCell>(
                  x_min, x_max, y_min, y_max, resolution),
              m_parent(parent)
        {
        }
        ~CCollisionGrid() override = default;
        /** Save to file, true = OK */
        bool saveToFile(
            mrpt::serialization::CArchive* fil,
            const mrpt::math::CPolygon&    computed_robotShape) const;
        /** Load from file,  true = OK */
        bool loadFromFile(
            mrpt::serialization::CArchive* fil,
            const mrpt::math::CPolygon&    current_robotShape);

        /** For an obstacle (x,y), returns a vector with all the pairs (a,d)
         * such as the robot collides */
        const TCollisionCell& getTPObstacle(
            const float obsX, const float obsY) const;

        /** 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:
         *      \param cellInfo The index of the cell
         * \param k The path index (alpha discreet value)
         * \param d The distance (in TP-Space, range 0..1) to collision.
         */
        void updateCellInfo(
            const unsigned int icx, const unsigned int icy, const uint16_t k,
            const float dist);

    };  // end of class CCollisionGrid

    // Save/Load from files.
    bool saveColGridsToFile(
        const std::string&          filename,
        const mrpt::math::CPolygon& computed_robotShape) const;  // true = OK
    bool loadColGridsFromFile(
        const std::string&          filename,
        const mrpt::math::CPolygon& current_robotShape);  // true = OK

    /** The collision grid */
    CCollisionGrid m_collisionGrid;

    /** Specifies the min/max values for "k" and "n", respectively.
     * \sa m_lambdaFunctionOptimizer
     */
    struct TCellForLambdaFunction
    {
        TCellForLambdaFunction()
            : k_min(std::numeric_limits<uint16_t>::max()),
              k_max(std::numeric_limits<uint16_t>::min()),
              n_min(std::numeric_limits<uint32_t>::max()),
              n_max(std::numeric_limits<uint32_t>::min())
        {
        }

        uint16_t k_min, k_max;
        uint32_t n_min, n_max;

        bool isEmpty() const
        {
            return k_min == std::numeric_limits<uint16_t>::max();
        }
    };

    /** This grid will contain indexes data for speeding-up the default,
     * brute-force lambda function */
    mrpt::containers::CDynamicGrid<TCellForLambdaFunction>
        m_lambdaFunctionOptimizer;
};

}  // namespace mpp::ptg

namespace mrpt::typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NAMESPACE(TCPoint, mpp::ptg)
}  // namespace mrpt::typemeta