PlannerRRT_common.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/math/CPolygon.h>
#include <mrpt/nav/planners/PlannerRRT_common.h>
#include <mrpt/nav/tpspace/CPTG_DiffDrive_CollisionGridBased.h>

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

RRTAlgorithmParams::RRTAlgorithmParams()
    : ptg_cache_files_directory("."),

      minAngBetweenNewNodes(mrpt::DEG2RAD(15))

{
    robot_shape.push_back(mrpt::math::TPoint2D(-0.5, -0.5));
    robot_shape.push_back(mrpt::math::TPoint2D(0.8, -0.4));
    robot_shape.push_back(mrpt::math::TPoint2D(0.8, 0.4));
    robot_shape.push_back(mrpt::math::TPoint2D(-0.5, 0.5));
}

PlannerTPS_VirtualBase::PlannerTPS_VirtualBase() = default;
void PlannerTPS_VirtualBase::internal_initialize_PTG()
{
    ASSERTMSG_(
        !m_PTGs.empty(),
        "No PTG was defined! At least one must be especified.");

    // Convert to CPolygon for API requisites:
    mrpt::math::CPolygon poly_robot_shape;
    poly_robot_shape.clear();
    if (!params.robot_shape.empty())
    {
        vector<double> xm, ym;
        params.robot_shape.getPlotData(xm, ym);
        poly_robot_shape.setAllVertices(xm, ym);
    }

    for (size_t i = 0; i < m_PTGs.size(); i++)
    {
        mrpt::system::CTimeLoggerEntry tle(m_timelogger, "PTG_initialization");

        // Polygonal robot shape?
        {
            auto* diff_ptg =
                dynamic_cast<mrpt::nav::CPTG_DiffDrive_CollisionGridBased*>(
                    m_PTGs[i].get());
            if (diff_ptg)
            {
                ASSERTMSG_(
                    !poly_robot_shape.empty(),
                    "No polygonal robot shape specified, and PTG requires "
                    "one!");
                diff_ptg->setRobotShape(poly_robot_shape);
            }
        }
        // Circular robot shape?
        {
            auto* ptg = dynamic_cast<mrpt::nav::CPTG_RobotShape_Circular*>(
                m_PTGs[i].get());
            if (ptg)
            {
                ASSERTMSG_(
                    params.robot_shape_circular_radius > 0,
                    "No circular robot shape specified, and PTG requires one!");
                ptg->setRobotShapeRadius(params.robot_shape_circular_radius);
            }
        }

        m_PTGs[i]->initialize(
            mrpt::format(
                "%s/TPRRT_PTG_%03u.dat.gz",
                params.ptg_cache_files_directory.c_str(),
                static_cast<unsigned int>(i)),
            params.ptg_verbose);
    }

    m_initialized_PTG = true;
}

void PlannerTPS_VirtualBase::internal_loadConfig_PTG(
    const mrpt::config::CConfigFileBase& ini, const std::string& sSect)
{
    // Robot shape:
    // ==========================
    // polygonal shape
    {
        // Robot shape is a bit special to load:
        params.robot_shape.clear();
        const std::string sShape = ini.read_string(sSect, "robot_shape", "");
        if (!sShape.empty())
        {
            CMatrixDouble mShape;
            if (!mShape.fromMatlabStringFormat(sShape))
                THROW_EXCEPTION_FMT(
                    "Error parsing robot_shape matrix: '%s'", sShape.c_str());
            ASSERT_(mShape.rows() == 2);
            ASSERT_(mShape.cols() >= 3);

            for (int i = 0; i < mShape.cols(); i++)
                params.robot_shape.push_back(
                    TPoint2D(mShape(0, i), mShape(1, i)));
        }
    }
    // circular shape
    params.robot_shape_circular_radius =
        ini.read_double(sSect, "robot_shape_circular_radius", 0.0);

    // Load PTG tables:
    // ==========================
    m_PTGs.clear();

    const size_t PTG_COUNT =
        ini.read_int(sSect, "PTG_COUNT", 0, true);  // load the number of PTGs
    for (unsigned int n = 0; n < PTG_COUNT; n++)
    {
        // Generate it:
        const std::string sPTGName =
            ini.read_string(sSect, format("PTG%u_Type", n), "", true);
        m_PTGs.push_back(CParameterizedTrajectoryGenerator::Ptr(
            CParameterizedTrajectoryGenerator::CreatePTG(
                sPTGName, ini, sSect, format("PTG%u_", n))));
    }
}

// Auxiliary function:
void PlannerTPS_VirtualBase::transformPointcloudWithSquareClipping(
    const mrpt::maps::CPointsMap& in_map, mrpt::maps::CPointsMap& out_map,
    const mrpt::poses::CPose2D& asSeenFrom, const double MAX_DIST_XY)
{
    size_t nObs;
    const float *obs_xs, *obs_ys, *obs_zs;
    in_map.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);

    out_map.clear();
    out_map.reserve(nObs);  // Prealloc mem for speed-up

    const CPose2D invPose = -asSeenFrom;
    // We can safely discard the rest of obstacles, since they cannot be
    // converted into TP-Obstacles anyway!

    for (size_t obs = 0; obs < nObs; obs++)
    {
        const double gx = obs_xs[obs], gy = obs_ys[obs];

        if (std::abs(gx - asSeenFrom.x()) > MAX_DIST_XY ||
            std::abs(gy - asSeenFrom.y()) > MAX_DIST_XY)
            continue;  // ignore this obstacle: anyway, I don't know how to map
        // it to TP-Obs!

        double ox, oy;
        invPose.composePoint(gx, gy, ox, oy);

        out_map.insertPointFast(ox, oy, 0);
    }
}

/*---------------------------------------------------------------
SpaceTransformer
---------------------------------------------------------------*/
void PlannerTPS_VirtualBase::spaceTransformer(
    const mrpt::maps::CSimplePointsMap& in_obstacles,
    const mrpt::nav::CParameterizedTrajectoryGenerator* in_PTG,
    const double MAX_DIST, std::vector<double>& out_TPObstacles)
{
    using namespace mrpt::nav;
    try
    {
        // Take "k_rand"s and "distances" such that the collision hits the
        // obstacles
        // in the "grid" of the given PT
        // --------------------------------------------------------------------
        size_t nObs;
        const float *obs_xs, *obs_ys, *obs_zs;
        // = in_obstacles.getPointsCount();
        in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);

        // Init obs ranges:
        in_PTG->initTPObstacles(out_TPObstacles);

        for (size_t obs = 0; obs < nObs; obs++)
        {
            const float ox = obs_xs[obs];
            const float oy = obs_ys[obs];

            if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
                continue;  // ignore this obstacle: anyway, I don't know how to
            // map it to TP-Obs!

            in_PTG->updateTPObstacle(ox, oy, out_TPObstacles);
        }

        // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
        // just represent real distances in meters.
    }
    catch (const std::exception& e)
    {
        cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
        cerr << e.what() << endl;
    }
    catch (...)
    {
        cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
        cerr << format("*in_PTG = %p\n", (void*)in_PTG);
        if (in_PTG)
            cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
        cerr << endl;
    }
}

void PlannerTPS_VirtualBase::spaceTransformerOneDirectionOnly(
    const int tp_space_k_direction,
    const mrpt::maps::CSimplePointsMap& in_obstacles,
    const mrpt::nav::CParameterizedTrajectoryGenerator* in_PTG,
    const double MAX_DIST, double& out_TPObstacle_k)
{
    using namespace mrpt::nav;
    try
    {
        // Take "k_rand"s and "distances" such that the collision hits the
        // obstacles
        // in the "grid" of the given PT
        // --------------------------------------------------------------------
        size_t nObs;
        const float *obs_xs, *obs_ys, *obs_zs;
        // = in_obstacles.getPointsCount();
        in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);

        // Init obs ranges:
        in_PTG->initTPObstacleSingle(tp_space_k_direction, out_TPObstacle_k);

        for (size_t obs = 0; obs < nObs; obs++)
        {
            const float ox = obs_xs[obs];
            const float oy = obs_ys[obs];

            if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
                continue;  // ignore this obstacle: anyway, I don't know how to
            // map it to TP-Obs!

            in_PTG->updateTPObstacleSingle(
                ox, oy, tp_space_k_direction, out_TPObstacle_k);
        }

        // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
        // just represent real distances in meters.
    }
    catch (const std::exception& e)
    {
        cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
        cerr << e.what() << endl;
    }
    catch (...)
    {
        cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
        cerr << format("*in_PTG = %p\n", (void*)in_PTG);
        if (in_PTG)
            cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
        cerr << endl;
    }
}