PTGs_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, 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 <gtest/gtest.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/system/filesystem.h>
#include <test_mrpt_common.h>

TEST(NavTests, PTGs_tests)
{
    using namespace std;
    using namespace mrpt;
    using namespace mrpt::nav;

    const string sFil =
        mrpt::UNITTEST_BASEDIR + string("/tests/PTGs_for_tests.ini");
    if (!mrpt::system::fileExists(sFil))
    {
        cerr << "**WARNING* Skipping tests since file cannot be found: '"
             << sFil << "'\n";
        return;
    }

    mrpt::config::CConfigFile cfg(sFil);

    const unsigned int PTG_COUNT =
        cfg.read_int("PTG_UNIT_TESTS", "PTG_COUNT", 0, true);
    EXPECT_TRUE(PTG_COUNT > 0);
    vector<CParameterizedTrajectoryGenerator::Ptr> PTGs(PTG_COUNT);

    for (unsigned int n = 0; n < PTG_COUNT; n++)
    {
        // Factory:
        const string sPTGName = cfg.read_string(
            "PTG_UNIT_TESTS", format("PTG%u_Type", n), "", true);
        PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(
            sPTGName, cfg, "PTG_UNIT_TESTS", format("PTG%u_", n));
        EXPECT_TRUE(PTGs[n] != nullptr) << "Failed creating PTG #" << n << endl;

        try
        {
            PTGs[n]->initialize(string(), false /*verbose */);
        }
        catch (const std::exception& e)
        {
            GTEST_FAIL() << "Failed initializing PTG #" << n << endl
                         << e.what() << endl;
        }
    }

    // Run tests:
    // ---------------------------------------------------------
    for (unsigned int n = 0; n < PTG_COUNT; n++)
    {
        CParameterizedTrajectoryGenerator::Ptr ptg = PTGs[n];

        const std::string sPTGDesc = ptg->getDescription();
        const double refDist = ptg->getRefDistance();
        const size_t num_paths = ptg->getPathCount();
        size_t num_tests_run = 0;

        // TEST: step <-> dist match
        {
            for (double dist = 0.1; dist < refDist * 0.5; dist += 0.2)
            {
                bool any_good = false;
                for (size_t k = 0; k < num_paths; k++)
                {
                    uint32_t step;

                    if (ptg->getPathStepForDist(k, dist, step))
                    {
                        any_good = true;
                        double d = ptg->getPathDist(k, step);
                        EXPECT_NEAR(d, dist, 0.05)
                            << "Test: step <-> dist match\n PTG: " << sPTGDesc
                            << endl
                            << "dist:" << dist << endl;
                        num_tests_run++;
                    }
                }
                EXPECT_TRUE(any_good)
                    << "Test: step <-> dist match\n PTG: " << sPTGDesc << endl
                    << "dist:" << dist << endl;
            }
        }

        // TEST: inverseMap_WS2TP
        {
            bool any_ok = false;
            bool skip_this_ptg = false;
            for (double tx = -refDist * 0.5;
                 !skip_this_ptg && tx < refDist * 0.5; tx += 0.1)
            {
                for (double ty = -refDist * 0.5;
                     !skip_this_ptg && ty < refDist * 0.5; ty += 0.1)
                {
                    if (std::abs(tx) < 1e-2 && std::abs(ty) < 1e-2)
                        continue;  // TP-Space does not include the WS point
                    // (0,0) in its domain

                    const double tolerance_dist = std::max(
                        0.10, 10.0 * std::sqrt(tx * tx + ty * ty) * M_PI * 2 /
                                  ptg->getPathCount());

                    int k;
                    double normalized_d;
                    bool valid = ptg->inverseMap_WS2TP(tx, ty, k, normalized_d);
                    if (valid && normalized_d < 1.0)
                    {
                        any_ok = true;
                        // Now, do the inverse operation:
                        uint32_t step;
                        bool step_ok = ptg->getPathStepForDist(
                            k, normalized_d * refDist, step);
                        EXPECT_TRUE(step_ok)
                            << "PTG: " << sPTGDesc << endl
                            << "(tx,ty): " << tx << " " << ty << " k= " << k
                            << " normalized_d=" << normalized_d << endl;
                        if (step_ok)
                        {
                            mrpt::math::TPose2D pose;
                            ptg->getPathPose(k, step, pose);
                            EXPECT_NEAR(pose.x, tx, tolerance_dist)
                                << "Test: inverseMap_WS2TP\n PTG#" << n << ": "
                                << sPTGDesc << endl
                                << "(tx,ty): " << tx << " " << ty << " k= " << k
                                << " normalized_d=" << normalized_d << endl;
                            EXPECT_NEAR(pose.y, ty, tolerance_dist)
                                << "Test: inverseMap_WS2TP\n PTG#" << n << ": "
                                << sPTGDesc << endl
                                << "(tx,ty): " << tx << " " << ty << " k= " << k
                                << " normalized_d=" << normalized_d << endl;

                            if (std::abs(pose.x - tx) >= tolerance_dist ||
                                std::abs(pose.y - ty) >= tolerance_dist)
                                skip_this_ptg = true;
                            else
                                num_tests_run++;
                        }
                    }
                }
            }
            EXPECT_TRUE(any_ok) << "PTG: " << sPTGDesc << endl;
        }

        // TEST: TP_obstacles
        {
            bool skip_this_ptg = false;
            bool any_change_all = false;
            for (double ox = -refDist * 0.5;
                 !skip_this_ptg && ox < refDist * 0.5; ox += 0.1)
            {
                for (double oy = -refDist * 0.5;
                     !skip_this_ptg && oy < refDist * 0.5; oy += 0.1)
                {
                    if (std::abs(ox) < 1e-2 && std::abs(oy) < 1e-2)
                        continue;  // TP-Space does not include the WS point
                    // (0,0) in its domain

                    std::vector<double> TP_obstacles;
                    ptg->initTPObstacles(TP_obstacles);

                    const std::vector<double> TP_obstacles_org = TP_obstacles;
                    ptg->updateTPObstacle(ox, oy, TP_obstacles);

                    const bool any_change = (TP_obstacles_org != TP_obstacles);
                    if (any_change) any_change_all = true;
                    num_tests_run++;
                }
            }
            EXPECT_TRUE(any_change_all);
        }

        printf(
            "PTG `%50s` run %6u tests.\n", sPTGDesc.c_str(),
            (unsigned int)num_tests_run);

    }  // for each ptg
}