PlannerSimple2D_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/io/CFileGZInputStream.h>
#include <mrpt/nav/planners/PlannerSimple2D.h>
#include <mrpt/serialization/CArchive.h>
#include <test_mrpt_common.h>

TEST(PlannerSimple2D, findPath)
{
    using namespace std::string_literals;

    const auto fil = mrpt::UNITTEST_BASEDIR +
                     "/share/mrpt/datasets/2006-MalagaCampus.gridmap.gz"s;

    // Load the gridmap:
    mrpt::maps::COccupancyGridMap2D gridmap;

    {
        mrpt::io::CFileGZInputStream f(fil);
        auto arch = mrpt::serialization::archiveFrom(f);
        arch >> gridmap;
    }

    // Find path:
    mrpt::nav::PlannerSimple2D pathPlanning;
    pathPlanning.robotRadius = 0.30f;

    {
        std::deque<mrpt::math::TPoint2D> thePath;
        bool notFound;
        const mrpt::poses::CPose2D origin(20, -110, 0), target(90, 40, 0);
        pathPlanning.computePath(gridmap, origin, target, thePath, notFound);

        EXPECT_FALSE(notFound);
        EXPECT_EQ(thePath.size(), 416U);
        EXPECT_NEAR(thePath.at(0).x, origin.x(), 1.0);
        EXPECT_NEAR(thePath.at(0).y, origin.y(), 1.0);
        EXPECT_NEAR(thePath.back().x, target.x(), 1.0);
        EXPECT_NEAR(thePath.back().y, target.y(), 1.0);
    }
    {
        std::deque<mrpt::math::TPoint2D> thePath;
        bool notFound;
        const mrpt::poses::CPose2D origin(90, 40, 0), target(20, -110, 0);
        pathPlanning.computePath(
            gridmap, origin, target, thePath, notFound,
            300.0f /* Max. distance */);

        EXPECT_FALSE(notFound);
        EXPECT_EQ(thePath.size(), 416U);
        EXPECT_NEAR(thePath.at(0).x, origin.x(), 1.0);
        EXPECT_NEAR(thePath.at(0).y, origin.y(), 1.0);
        EXPECT_NEAR(thePath.back().x, target.x(), 1.0);
        EXPECT_NEAR(thePath.back().y, target.y(), 1.0);
    }

    {
        std::deque<mrpt::math::TPoint2D> thePath;
        bool notFound;
        const mrpt::poses::CPose2D origin(20, -110, 0), target(90, 40, 0);
        pathPlanning.computePath(
            gridmap, origin, target, thePath, notFound,
            10.0f /* Max. distance */);

        EXPECT_TRUE(notFound);
    }

    {
        std::deque<mrpt::math::TPoint2D> thePath;
        bool notFound;
        const mrpt::poses::CPose2D origin(20, -110, 0), target(900, 40, 0);
        pathPlanning.computePath(
            gridmap, origin, target, thePath, notFound,
            100.0f /* Max. distance */);

        EXPECT_TRUE(notFound);
        EXPECT_EQ(thePath.size(), 0U);
    }
}