PTGs_unittest.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 <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/utils/CConfigFile.h>
#include <mrpt/system/filesystem.h>
#include <gtest/gtest.h>

// Defined in tests/test_main.cpp
namespace mrpt
{
namespace utils
{
extern std::string MRPT_GLOBAL_UNITTEST_SRC_DIR;
}
}

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

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

        mrpt::utils::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*> 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 (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* 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

        // Clean up:
        for (unsigned int n = 0; n < PTG_COUNT; n++) delete PTGs[n];
}