From 8af2e4c23733f8b02a4d5f4af172e0745c9e8760 Mon Sep 17 00:00:00 2001 From: haider8645 Date: Tue, 19 Nov 2024 08:15:09 +0100 Subject: [PATCH] Add unit tests (#18) * Add unit tests for environmentxyztheta * Add boost::filesystem library (must be after ugv_nav4d) * Add unit tests * Cleanup headers * Use set locale to C for grid resolution parameter from cli for gui * Throw error if translation or angular velocity is 0.0 * Add unit tests for computed motions * Update readme for unit tests * Test costs calculation for only one motion * Check if final pose of full and sampled splines * Check validity of underlying values in generated trajectories * Use the ugv_nav4d target for tests * Use rock executable macro for tests * Add filesystem dependency to all ugv_nav4d dependent tests --------- Co-authored-by: Mulo01 Co-authored-by: Christoph Hertzberg --- README.md | 11 +- src/Mobility.hpp | 22 +- src/PreComputedMotions.cpp | 7 +- src/test/CMakeLists.txt | 21 +- src/test/test_DiscreteTheta.cpp | 38 ++++ src/test/test_EnvironmentXYZTheta.cpp | 314 ++++++++++++++++++-------- src/test/test_Planner.cpp | 264 ++++++++++++++++++++++ src/test/test_PreComputedMotions.cpp | 153 +++++++++++++ src/test/test_ugv_nav4d.cpp | 274 ---------------------- 9 files changed, 709 insertions(+), 395 deletions(-) create mode 100644 src/test/test_DiscreteTheta.cpp create mode 100644 src/test/test_Planner.cpp create mode 100644 src/test/test_PreComputedMotions.cpp delete mode 100644 src/test/test_ugv_nav4d.cpp diff --git a/README.md b/README.md index 2209eed..d767049 100644 --- a/README.md +++ b/README.md @@ -106,17 +106,8 @@ cd build cmake -DCMAKE_INSTALL_PREFIX=./install -DTESTS_ENABLED=ON -DENABLE_DEBUG_DRAWINGS=OFF -DCMAKE_BUILD_TYPE=RELEASE .. make install ``` -Run the unit tests using the executable -``` -test_ugv_nav4d ../test_data/Plane1Mio.ply -``` -At the end you should see the output -``` -[----------] Global test environment tear-down -[==========] 6 tests from 2 test suites ran. (10297 ms total) -[ PASSED ] 6 tests. -``` +The test executables are in the folder: `build/src/test/`. --- # ROS 2 Humble Test Environment with Gazebo Fortress diff --git a/src/Mobility.hpp b/src/Mobility.hpp index d96a35d..5196727 100644 --- a/src/Mobility.hpp +++ b/src/Mobility.hpp @@ -43,19 +43,19 @@ struct Mobility { Mobility() : translationSpeed(1.0), rotationSpeed(1.0), - minTurningRadius(0.0), - spline_sampling_resolution(0.01), - remove_goal_offset(true), + minTurningRadius(1.0), + spline_sampling_resolution(0.05), + remove_goal_offset(false), multiplierForward(1), - multiplierBackward(1), - multiplierLateral(1), - multiplierForwardTurn(1), - multiplierBackwardTurn(1), - multiplierPointTurn(1), - multiplierLateralCurve(1), - searchRadius(2.0), + multiplierBackward(2), + multiplierLateral(4), + multiplierForwardTurn(2), + multiplierBackwardTurn(3), + multiplierPointTurn(3), + multiplierLateralCurve(4), + searchRadius(1.0), searchProgressSteps(0.1), - maxMotionCurveLength(1.2) + maxMotionCurveLength(100) { } diff --git a/src/PreComputedMotions.cpp b/src/PreComputedMotions.cpp index 4aeb0f0..9856b91 100644 --- a/src/PreComputedMotions.cpp +++ b/src/PreComputedMotions.cpp @@ -296,6 +296,11 @@ void PreComputedMotions::computeSplinePrimCost(const SplinePrimitive& prim, int Motion::calculateCost(double translationalDist, double angularDist, double translationVelocity, double angularVelocity, double costMultiplier) { + if (translationVelocity == 0.0 || angularVelocity == 0.0) { + LOG_ERROR_S << "ERROR calculateCost: Division by zero translation or angular velocity."; + throw std::runtime_error("ERROR calculateCost: Division by zero translation or angular velocity."); + + } const double translationTime = translationalDist / translationVelocity; const double angularTime = angularDist / angularVelocity; @@ -304,7 +309,7 @@ int Motion::calculateCost(double translationalDist, double angularDist, double t if(cost > std::numeric_limits::max()) { - std::cerr << "WARNING: primitive cost too large for int. Clipping to int_max." << std::endl; + LOG_ERROR_S << "WARNING: primitive cost too large for int. Clipping to int_max."; return std::numeric_limits::max(); } else diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index c23ae08..a84e31d 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -1,13 +1,24 @@ -find_package(Boost REQUIRED COMPONENTS filesystem) +find_package(Boost REQUIRED COMPONENTS filesystem serialization) - -rock_executable(test_ugv_nav4d - SOURCES test_ugv_nav4d.cpp +rock_executable(test_Planner + SOURCES test_Planner.cpp + DEPS Boost::filesystem DEPS_PKGCONFIG ugv_nav4d ) rock_executable(test_EnvironmentXYZTheta SOURCES test_EnvironmentXYZTheta.cpp - DEPS Boost::filesystem + DEPS Boost::filesystem Boost::serialization + DEPS_PKGCONFIG ugv_nav4d +) + +rock_executable(test_DiscreteTheta + SOURCES test_DiscreteTheta.cpp + DEPS_PKGCONFIG ugv_nav4d +) + +rock_executable(test_PreComputedMotions + SOURCES test_PreComputedMotions.cpp + DEPS Boost::filesystem DEPS_PKGCONFIG ugv_nav4d ) diff --git a/src/test/test_DiscreteTheta.cpp b/src/test/test_DiscreteTheta.cpp new file mode 100644 index 0000000..4004e3e --- /dev/null +++ b/src/test/test_DiscreteTheta.cpp @@ -0,0 +1,38 @@ +#define BOOST_TEST_MODULE DiscreteThetaModule +#include + +#include "ugv_nav4d/DiscreteTheta.hpp" + +BOOST_AUTO_TEST_CASE(check_discrete_theta_init) { + DiscreteTheta theta = DiscreteTheta(0, 16); + BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 0, 0.01); + + theta = DiscreteTheta(1, 16); + BOOST_CHECK_EQUAL(theta.getNumAngles(), 16); + BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 0.3926, 0.01); + + theta = DiscreteTheta(-1, 16); + BOOST_CHECK_EQUAL(theta.getTheta(), 15); + BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 5.8904, 0.01); + + theta = DiscreteTheta(18, 16); + BOOST_CHECK_EQUAL(theta.getTheta(), 2); + + theta = DiscreteTheta(16, 16); + BOOST_CHECK_EQUAL(theta.getTheta(), 0); + + theta = DiscreteTheta(5.90, 16); + BOOST_CHECK_EQUAL(theta.getTheta(), 15); + + theta = DiscreteTheta(5.45, 16); + BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 5.45, 0.01); + BOOST_CHECK_EQUAL(theta.getTheta(), 14); + + DiscreteTheta thetaA = DiscreteTheta(3, 16); + DiscreteTheta thetaB = DiscreteTheta(5, 16); + BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 2); + + thetaA = DiscreteTheta(2, 16); + thetaB = DiscreteTheta(14, 16); + BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 4); +} \ No newline at end of file diff --git a/src/test/test_EnvironmentXYZTheta.cpp b/src/test/test_EnvironmentXYZTheta.cpp index b920017..8c8a727 100644 --- a/src/test/test_EnvironmentXYZTheta.cpp +++ b/src/test/test_EnvironmentXYZTheta.cpp @@ -1,130 +1,256 @@ #define BOOST_TEST_MODULE EnvironmentXYZThetaTestModule - -#include -#include - -#include "ugv_nav4d/EnvironmentXYZTheta.hpp" -#include "ugv_nav4d/Mobility.hpp" -#include -#include - -#include - -#include -#include -#include - #include #include #include +#include "ugv_nav4d/EnvironmentXYZTheta.hpp" + using namespace ugv_nav4d; +using namespace maps::grid; -std::string mlsBinFile; +std::vector startPositions; +std::vector goalPositions; -// Define EnvironmentXYZThetaTest fixture for Boost Test struct EnvironmentXYZThetaTest { - EnvironmentXYZThetaTest(); - ~EnvironmentXYZThetaTest(); + EnvironmentXYZThetaTest(){} + ~EnvironmentXYZThetaTest(){} - void loadMlsMap(); - - typedef EnvironmentXYZTheta::MLGrid MLSBase; EnvironmentXYZTheta* environment; - maps::grid::MLSMapSloped mlsMap; Mobility mobility; sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; traversability_generator3d::TraversabilityConfig traversabilityConfig; bool map_loaded = false; }; -EnvironmentXYZThetaTest::EnvironmentXYZThetaTest() { - std::cout << "Called SetUp()" << std::endl; +BOOST_FIXTURE_TEST_CASE(travmap_resolution_not_equal_to_mls_resolution, EnvironmentXYZThetaTest) { + Vector2d res(0.3, 0.3); + Vector2ui numCells(10, 10); - splinePrimitiveConfig.gridSize = 0.3; - splinePrimitiveConfig.numAngles = 16; - splinePrimitiveConfig.numEndAngles = 8; - splinePrimitiveConfig.destinationCircleRadius = 6; - splinePrimitiveConfig.cellSkipFactor = 0.1; - splinePrimitiveConfig.splineOrder = 4.0; - - mobility.translationSpeed = 0.5; - mobility.rotationSpeed = 0.5; - mobility.minTurningRadius = 1; - mobility.spline_sampling_resolution = 0.05; - mobility.remove_goal_offset = true; - mobility.multiplierForward = 1; - mobility.multiplierBackward = 3; - mobility.multiplierPointTurn = 3; - mobility.multiplierLateral = 4; - mobility.multiplierForwardTurn = 2; - mobility.multiplierBackwardTurn = 4; - mobility.multiplierLateralCurve = 4; - mobility.searchRadius = 1.0; - mobility.searchProgressSteps = 0.1; - mobility.maxMotionCurveLength = 100; - - traversabilityConfig.maxStepHeight = 0.25; - traversabilityConfig.maxSlope = 0.45; - traversabilityConfig.inclineLimittingMinSlope = 0.2; - traversabilityConfig.inclineLimittingLimit = 0.1; - traversabilityConfig.costFunctionDist = 0.0; - traversabilityConfig.minTraversablePercentage = 0.4; - traversabilityConfig.robotHeight = 1.2; - traversabilityConfig.robotSizeX = 1.35; - traversabilityConfig.robotSizeY = 0.85; - traversabilityConfig.distToGround = 0.0; - traversabilityConfig.slopeMetricScale = 1.0; - traversabilityConfig.slopeMetric = traversability_generator3d::NONE; + MLSConfig mls_config; + mls_config.gapSize = 0.1; + mls_config.updateModel = MLSConfig::SLOPE; + MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); + + /** Translate the local frame (offset) **/ + mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; + + Eigen::Vector2d max = 0.5 * mls_o.getSize(); + Eigen::Vector2d min = -0.5 * mls_o.getSize(); + for (double x = min.x(); x < max.x(); x += 0.00625) + { + //double cs = std::cos(x * M_PI/2.5); + for (double y = min.y(); y < max.y(); y += 0.00625) + { + //double sn = std::sin(y * M_PI/2.5); + mls_o.mergePoint(Eigen::Vector3d(x, y, 0)); + } + } + + std::shared_ptr mlsPtr = std::make_shared(mls_o); + traversabilityConfig.gridResolution = 0.5; + splinePrimitiveConfig.gridSize = 0.5; + environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); + std::vector startPositions; + startPositions.emplace_back(Eigen::Vector3d(0,0,0)); + environment->expandMap(startPositions); + BOOST_CHECK_EQUAL(environment->getTravGen().getNumNodes(), 1); + delete environment; + + traversabilityConfig.gridResolution = 0.1; + splinePrimitiveConfig.gridSize = 0.1; + environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); + environment->expandMap(startPositions); + BOOST_CHECK_EQUAL(environment->getTravGen().getNumNodes(), 9); +} + +BOOST_FIXTURE_TEST_CASE(travmap_resolution_equal_to_mls_resolution, EnvironmentXYZThetaTest) { + Vector2d res(0.3, 0.3); + Vector2ui numCells(10, 10); + + MLSConfig mls_config; + mls_config.gapSize = 0.1; + mls_config.updateModel = MLSConfig::SLOPE; + MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); + + /** Translate the local frame (offset) **/ + mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; + + Eigen::Vector2d max = 0.5 * mls_o.getSize(); + Eigen::Vector2d min = -0.5 * mls_o.getSize(); + + for (double x = min.x(); x < max.x(); x += 0.00625) + { + //double cs = std::cos(x * M_PI/2.5); + for (double y = min.y(); y < max.y(); y += 0.00625) + { + //double sn = std::sin(y * M_PI/2.5); + mls_o.mergePoint(Eigen::Vector3d(x, y, 0)); + } + } + + std::shared_ptr mlsPtr = std::make_shared(mls_o); traversabilityConfig.gridResolution = 0.3; - traversabilityConfig.initialPatchVariance = 0.0001; - traversabilityConfig.allowForwardDownhill = true; - traversabilityConfig.enableInclineLimitting = false; + splinePrimitiveConfig.gridSize = 0.3; - loadMlsMap(); + environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); + std::vector startPositions; + startPositions.emplace_back(Eigen::Vector3d(0,0,0)); + environment->expandMap(startPositions); + BOOST_CHECK_EQUAL(environment->getTravGen().getNumNodes(), numCells.x()* numCells.y()); } -EnvironmentXYZThetaTest::~EnvironmentXYZThetaTest(){ - std::cout << "Called TearDown()" << std::endl; - delete environment; +BOOST_FIXTURE_TEST_CASE(check_travmap, EnvironmentXYZThetaTest) { + Vector2d res(0.3, 0.3); + Vector2ui numCells(10, 10); + + MLSConfig mls_config; + mls_config.gapSize = 0.1; + mls_config.updateModel = MLSConfig::SLOPE; + MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); + + /** Translate the local frame (offset) **/ + mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; + + Eigen::Vector2d max = 0.5 * mls_o.getSize(); + Eigen::Vector2d min = -0.5 * mls_o.getSize(); + + for (double x = min.x(); x < max.x(); x += 0.00625) + { + for (double y = min.y(); y < max.y(); y += 0.00625) + { + double z = 0; + if ((x >= 0.6 && x < 0.9) && (y >= 0.6 && y < 0.9)){ + z = 0.3; + } + mls_o.mergePoint(Eigen::Vector3d(x, y, z)); + } + } + + std::shared_ptr mlsPtr = std::make_shared(mls_o); + traversabilityConfig.gridResolution = 0.3; + splinePrimitiveConfig.gridSize = 0.3; + + environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); + std::vector startPositions; + startPositions.emplace_back(Eigen::Vector3d(0,0,0)); + environment->expandMap(startPositions); + + Eigen::Vector3d positionFron{0.9, 0.3, 0}; + maps::grid::Index idxFrontierNode; + environment->getTravGen().getTraversabilityMap().toGrid(positionFron, idxFrontierNode); + auto frontier = environment->getTravGen().findMatchingTraversabilityPatchAt(idxFrontierNode,0); + BOOST_CHECK_EQUAL(frontier->getType(), ::maps::grid::TraversabilityNodeBase::FRONTIER); + + Eigen::Vector3d positionTrav{0.3, 0.3, 0}; + maps::grid::Index idxTraversableNode; + environment->getTravGen().getTraversabilityMap().toGrid(positionTrav, idxTraversableNode); + auto traversable = environment->getTravGen().findMatchingTraversabilityPatchAt(idxTraversableNode,0); + BOOST_CHECK_EQUAL(traversable->getType(), ::maps::grid::TraversabilityNodeBase::TRAVERSABLE); + + Eigen::Vector3d positionObs{0.65, 0.65, 0}; + maps::grid::Index idxObstacleNode; + environment->getTravGen().getTraversabilityMap().toGrid(positionObs, idxObstacleNode); + auto &trList(environment->getTravGen().getTraversabilityMap().at(idxObstacleNode)); + for(auto *snode : trList) + { + BOOST_CHECK_EQUAL(snode->getType(), ::maps::grid::TraversabilityNodeBase::OBSTACLE); + } } -void EnvironmentXYZThetaTest::loadMlsMap() { - std::string filename; +BOOST_FIXTURE_TEST_CASE(check_stepheight, EnvironmentXYZThetaTest) { + Vector2d res(0.3, 0.3); + Vector2ui numCells(10, 10); + + MLSConfig mls_config; + mls_config.gapSize = 0.1; + mls_config.updateModel = MLSConfig::SLOPE; + MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); + + /** Translate the local frame (offset) **/ + mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; + + Eigen::Vector2d max = 0.5 * mls_o.getSize(); + Eigen::Vector2d min = -0.5 * mls_o.getSize(); - if (mlsBinFile.empty()) { - filename = "cave_circuit_mls.bin"; - } else { - filename = mlsBinFile; + for (double x = min.x(); x < max.x(); x += 0.00625) + { + for (double y = min.y(); y < max.y(); y += 0.00625) + { + double z = 0; + if ((x >= 0 && x < 0.3) && (y >= 0 && y < 0.3)){ + z = 0.1; + } + mls_o.mergePoint(Eigen::Vector3d(x, y, z)); + } } - std::ifstream file(filename, std::ios::binary); - if (!file.is_open()) { - map_loaded = false; - return; + std::shared_ptr mlsPtr = std::make_shared(mls_o); + traversabilityConfig.gridResolution = 0.3; + splinePrimitiveConfig.gridSize = 0.3; + + environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); + std::vector startPositions; + startPositions.emplace_back(Eigen::Vector3d(0,0,0)); + environment->expandMap(startPositions); + + Eigen::Vector3d positionObs{0.25, 0.25, 0}; + maps::grid::Index idxObstacleNode; + + environment->getTravGen().getTraversabilityMap().toGrid(positionObs, idxObstacleNode); + for(auto *snode : environment->getTravGen().getTraversabilityMap().at(idxObstacleNode)) + { + BOOST_CHECK_EQUAL(snode->getType(), ::maps::grid::TraversabilityNodeBase::OBSTACLE); } - try { - boost::archive::binary_iarchive ia(file); - ia >> mlsMap; // Deserialize into mlsMap - } catch (const std::exception& e) { - map_loaded = false; + delete environment; + + traversabilityConfig.maxStepHeight = 0.2; + environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); + environment->expandMap(startPositions); + + environment->getTravGen().getTraversabilityMap().toGrid(positionObs, idxObstacleNode); + for(auto *snode : environment->getTravGen().getTraversabilityMap().at(idxObstacleNode)) + { + BOOST_CHECK_EQUAL(snode->getType(), ::maps::grid::TraversabilityNodeBase::TRAVERSABLE); } - map_loaded = true; } -// Define the test suite and test cases -BOOST_FIXTURE_TEST_SUITE(EnvironmentXYZThetaTestSuite, EnvironmentXYZThetaTest) +BOOST_FIXTURE_TEST_CASE(set_start_and_goal, EnvironmentXYZThetaTest) { + Vector2d res(0.3, 0.3); + Vector2ui numCells(100, 100); + + MLSConfig mls_config; + mls_config.gapSize = 0.1; + mls_config.updateModel = MLSConfig::SLOPE; + MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); + + /** Translate the local frame (offset) **/ + mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; + + Eigen::Vector2d max = 0.5 * mls_o.getSize(); + Eigen::Vector2d min = -0.5 * mls_o.getSize(); -BOOST_AUTO_TEST_CASE(check_planner_init_failure_wrong_grid_resolutions) { - std::shared_ptr mlsPtr = std::make_shared(mlsMap); - try { - environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); - } catch (const std::exception& ex) { - std::cout << "Exception: \n" << ex.what() << "\n"; - BOOST_CHECK_NE(splinePrimitiveConfig.gridSize, traversabilityConfig.gridResolution); + for (double x = min.x(); x < max.x(); x += 0.00625) + { + for (double y = min.y(); y < max.y(); y += 0.00625) + { + mls_o.mergePoint(Eigen::Vector3d(x, y, 0)); + } } -} -BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file + std::shared_ptr mlsPtr = std::make_shared(mls_o); + traversabilityConfig.gridResolution = 0.3; + splinePrimitiveConfig.gridSize = 0.3; + + environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); + std::vector startPositions; + startPositions.emplace_back(Eigen::Vector3d(0,0,0)); + environment->expandMap(startPositions); + + Eigen::Vector3d startPos{0,0,0}; + Eigen::Vector3d goalPos{12, 12, 0}; + + environment->setStart(startPos,0); + environment->setGoal(goalPos,0); +} diff --git a/src/test/test_Planner.cpp b/src/test/test_Planner.cpp new file mode 100644 index 0000000..967312d --- /dev/null +++ b/src/test/test_Planner.cpp @@ -0,0 +1,264 @@ +#define BOOST_TEST_MODULE PlannerTestModule +#include + +#include "ugv_nav4d/Planner.hpp" +#include + +using namespace ugv_nav4d; + +class PlannerTest { +public: + PlannerTest(); + ~PlannerTest(); + std::string getResult(const Planner::PLANNING_RESULT& result); + + Planner* planner; + maps::grid::MLSMapSloped* mlsMap; + PlannerConfig plannerConfig; + Mobility mobility; + sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; + traversability_generator3d::TraversabilityConfig traversabilityConfig; +}; + +PlannerTest::PlannerTest() { + + splinePrimitiveConfig.gridSize = 0.3; + splinePrimitiveConfig.numAngles = 16; + splinePrimitiveConfig.numEndAngles = 8; + splinePrimitiveConfig.destinationCircleRadius = 6; + splinePrimitiveConfig.cellSkipFactor = 0.1; + splinePrimitiveConfig.splineOrder = 4.0; + + mobility.translationSpeed = 0.5; + mobility.rotationSpeed = 0.5; + mobility.minTurningRadius = 1; + mobility.spline_sampling_resolution = 0.05; + mobility.remove_goal_offset = true; + mobility.multiplierForward = 1; + mobility.multiplierBackward = 2; + mobility.multiplierPointTurn = 3; + mobility.multiplierLateral = 4; + mobility.multiplierForwardTurn = 2; + mobility.multiplierBackwardTurn = 3; + mobility.multiplierLateralCurve = 4; + mobility.searchRadius = 1.0; + mobility.searchProgressSteps = 0.1; + mobility.maxMotionCurveLength = 100; + + traversabilityConfig.maxStepHeight = 0.2; + traversabilityConfig.maxSlope = 0.45; + traversabilityConfig.inclineLimittingMinSlope = 0.2; + traversabilityConfig.inclineLimittingLimit = 0.1; + traversabilityConfig.costFunctionDist = 0.0; + traversabilityConfig.minTraversablePercentage = 0.4; + traversabilityConfig.robotHeight = 0.5; + traversabilityConfig.robotSizeX = 0.5; + traversabilityConfig.robotSizeY = 0.5; + traversabilityConfig.distToGround = 0.0; + traversabilityConfig.slopeMetricScale = 1.0; + traversabilityConfig.slopeMetric = traversability_generator3d::NONE; + traversabilityConfig.gridResolution = 0.3; + traversabilityConfig.initialPatchVariance = 0.0001; + traversabilityConfig.allowForwardDownhill = true; + traversabilityConfig.enableInclineLimitting = false; + + plannerConfig.searchUntilFirstSolution = false; + plannerConfig.initialEpsilon = 64; + plannerConfig.epsilonSteps = 2; + plannerConfig.numThreads = 4; + + maps::grid::Vector2d res(0.3, 0.3); + maps::grid::Vector2ui numCells(100, 100); + + maps::grid::MLSConfig mls_config; + mls_config.gapSize = 0.1; + mls_config.updateModel = maps::grid::MLSConfig::SLOPE; + + mlsMap = new maps::grid::MLSMapSloped(numCells, res, mls_config); + + /** Translate the local frame (offset) **/ + mlsMap->getLocalFrame().translation() << 0.5*mlsMap->getSize(), 0; + + Eigen::Vector2d max = 0.5 * mlsMap->getSize(); + Eigen::Vector2d min = -0.5 * mlsMap->getSize(); + + for (double x = min.x(); x < max.x(); x += 0.00625) + { + for (double y = min.y(); y < max.y(); y += 0.00625) + { + mlsMap->mergePoint(Eigen::Vector3d(x, y, 0)); + } + } +} + +PlannerTest::~PlannerTest() { + delete planner; + delete mlsMap; +} + +std::string PlannerTest::getResult(const Planner::PLANNING_RESULT& result) { + switch (result) { + case Planner::GOAL_INVALID: return "GOAL_INVALID"; + case Planner::START_INVALID: return "START_INVALID"; + case Planner::NO_SOLUTION: return "NO_SOLUTION"; + case Planner::NO_MAP: return "NO_MAP"; + case Planner::INTERNAL_ERROR: return "INTERNAL_ERROR"; + case Planner::FOUND_SOLUTION: return "FOUND_SOLUTION"; + default: return "ERROR unknown result state"; + } +} + +BOOST_FIXTURE_TEST_SUITE(PlannerTestSuite, PlannerTest) + +BOOST_AUTO_TEST_CASE(check_planner_init_success) { + planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); + BOOST_CHECK(planner != nullptr); +} + +BOOST_AUTO_TEST_CASE(check_planner_goal_invalid) { + planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); + BOOST_REQUIRE(planner != nullptr); + planner->updateMap(*mlsMap); + + base::samples::RigidBodyState startState; + startState.position.x() = 0; + startState.position.y() = 0; + startState.position.z() = 0; + startState.orientation = Eigen::Quaterniond(1,0,0,0); + + base::samples::RigidBodyState endState; + endState.position.x() = 40.0; + endState.position.y() = 0.0; + endState.position.z() = 0.0; + endState.orientation = Eigen::Quaterniond(1,0,0,0); + + int maxTime = 5; + std::vector trajectory2D; + std::vector trajectory3D; + + const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); + BOOST_CHECK_EQUAL(result, Planner::GOAL_INVALID); +} + +BOOST_AUTO_TEST_CASE(check_planner_start_invalid) { + planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); + BOOST_REQUIRE(planner != nullptr); + planner->updateMap(*mlsMap); + + base::samples::RigidBodyState startState; + startState.position.x() = 40.0; + startState.position.y() = 0.0; + startState.position.z() = 0.0; + startState.orientation = Eigen::Quaterniond(1,0,0,0); + + base::samples::RigidBodyState endState; + endState.position.x() = 1.0; + endState.position.y() = 0.0; + endState.position.z() = 0.0; + endState.orientation = Eigen::Quaterniond(1,0,0,0); + + int maxTime = 5; + std::vector trajectory2D; + std::vector trajectory3D; + + const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); + BOOST_CHECK_EQUAL(result, Planner::START_INVALID); +} + +BOOST_AUTO_TEST_CASE(check_planner_success) { + planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); + BOOST_REQUIRE(planner != nullptr); + planner->updateMap(*mlsMap); + + base::samples::RigidBodyState startState; + startState.position.x() = 2.3; + startState.position.y() = 4.1; + startState.position.z() = 0.0; + startState.orientation = Eigen::Quaterniond(1,0,0,0); + + base::samples::RigidBodyState endState; + endState.position.x() = 6.1; + endState.position.y() = 4.2; + endState.position.z() = 0.0; + endState.orientation = Eigen::Quaterniond(1,0,0,0); + + int maxTime = 5; + std::vector trajectory2D; + std::vector trajectory3D; + + const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); + BOOST_CHECK_EQUAL(result, Planner::FOUND_SOLUTION); + BOOST_CHECK_GT(trajectory2D.size(), 0); + BOOST_CHECK_GT(trajectory3D.size(), 0); + BOOST_CHECK_EQUAL(trajectory2D.size(), trajectory3D.size()); + + //Start and Goal Positions + BOOST_REQUIRE(trajectory2D.front().startPose.position.isApprox(startState.position.head<2>(), 1e-6)); + BOOST_REQUIRE(trajectory2D.back().goalPose.position.isApprox(endState.position.head<2>(), 1e-6)); + BOOST_REQUIRE(trajectory2D.front().posSpline.getStartPoint().isApprox(startState.position, 1e-6)); + BOOST_REQUIRE(trajectory2D.back().posSpline.getEndPoint().isApprox(endState.position, 1e-6)); + + BOOST_REQUIRE(trajectory3D.front().startPose.position.isApprox(startState.position.head<2>(), 1e-6)); + BOOST_REQUIRE(trajectory3D.back().goalPose.position.isApprox(endState.position.head<2>(), 1e-6)); + BOOST_REQUIRE(trajectory3D.front().posSpline.getStartPoint().isApprox(startState.position, 1e-6)); + BOOST_REQUIRE(trajectory3D.back().posSpline.getEndPoint().isApprox(endState.position, 1e-6)); + + + //Start and Goal Orientations + BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory2D.front().startPose.orientation).getRad(), + base::Angle::fromRad(base::getYaw(startState.orientation)).getRad()); + BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory2D.back().goalPose.orientation).getRad(), + base::Angle::fromRad(base::getYaw(endState.orientation)).getRad()); + + BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory3D.front().startPose.orientation).getRad(), + base::Angle::fromRad(base::getYaw(startState.orientation)).getRad()); + BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory3D.back().goalPose.orientation).getRad(), + base::Angle::fromRad(base::getYaw(endState.orientation)).getRad()); + + for (auto& trajectory : trajectory2D){ + BOOST_REQUIRE(trajectory.speed <= mobility.translationSpeed); + BOOST_REQUIRE(trajectory.speed <= mobility.rotationSpeed); + + if (trajectory.driveMode != trajectory_follower::ModeTurnOnTheSpot){ + BOOST_REQUIRE(trajectory.posSpline.isEmpty() == false); + + BOOST_REQUIRE(trajectory.getCurvatureMax() <= mobility.minTurningRadius/2); + BOOST_REQUIRE(trajectory.posSpline.getCurveLength() <= mobility.maxMotionCurveLength); + } + else{ + BOOST_REQUIRE(trajectory.orientationSpline.isEmpty() == false); + + BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getStartParam()).orientation).getRad(), + base::Angle::fromRad(trajectory.startPose.orientation).getRad(), 1e-6); + + BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getEndParam()).orientation).getRad(), + base::Angle::fromRad(trajectory.goalPose.orientation).getRad(), 1e-6); + } + } + + for (auto& trajectory : trajectory3D){ + BOOST_REQUIRE(trajectory.speed <= mobility.translationSpeed); + BOOST_REQUIRE(trajectory.speed <= mobility.rotationSpeed); + + if (trajectory.driveMode != trajectory_follower::ModeTurnOnTheSpot){ + BOOST_REQUIRE(trajectory.posSpline.isEmpty() == false); + + BOOST_REQUIRE(trajectory.getCurvatureMax() <= mobility.minTurningRadius/2); + BOOST_REQUIRE(trajectory.posSpline.getCurveLength() <= mobility.maxMotionCurveLength); + } + else{ + BOOST_REQUIRE(trajectory.orientationSpline.isEmpty() == false); + + BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getStartParam()).orientation).getRad(), + base::Angle::fromRad(trajectory.startPose.orientation).getRad(), 1e-6); + + BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getEndParam()).orientation).getRad(), + base::Angle::fromRad(trajectory.goalPose.orientation).getRad(), 1e-6); + } + } + + +} + + +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/src/test/test_PreComputedMotions.cpp b/src/test/test_PreComputedMotions.cpp new file mode 100644 index 0000000..eff96fb --- /dev/null +++ b/src/test/test_PreComputedMotions.cpp @@ -0,0 +1,153 @@ +#define BOOST_TEST_MODULE PreComputedMotionsModule +#include + +#include "ugv_nav4d/PreComputedMotions.hpp" +#include + +using namespace ugv_nav4d; + +BOOST_AUTO_TEST_CASE(initialization) { + + sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; + Mobility mobilityConfig; + + PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); + delete motions; +} + +BOOST_AUTO_TEST_CASE(calculate_curvature_from_radius) { + + sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; + Mobility mobilityConfig; + + PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); + double curvature = motions->calculateCurvatureFromRadius(2.0); + BOOST_CHECK_CLOSE_FRACTION(curvature, 0.5, 0.001); + + delete motions; +} + +BOOST_AUTO_TEST_CASE(generate_motions) { + + sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; + Mobility mobilityConfig; + mobilityConfig.minTurningRadius = 1.0; + + PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); + motions->computeMotions(splinePrimitiveConfig.gridSize, splinePrimitiveConfig.gridSize); + delete motions; +} + +BOOST_AUTO_TEST_CASE(check_motions) { + + sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; + Mobility mobilityConfig; + mobilityConfig.minTurningRadius = 1.0; + + PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); + motions->computeMotions(splinePrimitiveConfig.gridSize, splinePrimitiveConfig.gridSize); + + //We generate primitives for 16 start angles. + for (int i = 1; i <= 16; ++i) { + DiscreteTheta theta(i,16); + std::vector motions_start = motions->getMotionForStartTheta(theta); + BOOST_REQUIRE(motions_start.size() > 0); + for (const auto& motion : motions_start) { + BOOST_REQUIRE(motion.baseCost > 0); + BOOST_REQUIRE(motion.costMultiplier > 0); + BOOST_REQUIRE(motion.costScaleFactor > 0); + + switch (motion.type){ + case Motion::MOV_FORWARD: + BOOST_CHECK_GT(motion.fullSplineSamples.size(), 0); + BOOST_CHECK_GT(motion.intermediateStepsTravMap.size(), 0); + BOOST_CHECK_GT(motion.intermediateStepsObstMap.size(), 0); + break; + case Motion::MOV_BACKWARD: + BOOST_CHECK_GT(motion.fullSplineSamples.size(), 0); + BOOST_CHECK_GT(motion.intermediateStepsTravMap.size(), 0); + BOOST_CHECK_GT(motion.intermediateStepsObstMap.size(), 0); + break; + case Motion::MOV_LATERAL: + BOOST_CHECK_GT(motion.fullSplineSamples.size(), 0); + BOOST_CHECK_GT(motion.intermediateStepsTravMap.size(), 0); + BOOST_CHECK_GT(motion.intermediateStepsObstMap.size(), 0); + BOOST_CHECK_EQUAL(motion.startTheta.getRadian(), + motion.endTheta.getRadian()); + break; + case Motion:: MOV_POINTTURN: + BOOST_CHECK_EQUAL(motion.fullSplineSamples.size(), 0); + BOOST_CHECK_NE(motion.startTheta.getRadian(), motion.endTheta.getRadian()); + break; + default: + throw std::runtime_error("test_PreComputedMotions: Invalid motion type detectd!"); + break; + + } + + //Point-turns do not have any splines + if (motion.type != Motion::MOV_POINTTURN){ + for (const auto& cellWithPoses : motion.fullSplineSamples){ + BOOST_CHECK_GT(cellWithPoses.poses.size(), 0); + } + + base::Angle splineStartAngle = base::Angle::fromRad(motion.fullSplineSamples.front().poses.front().orientation); + base::Angle splineEndAngle = base::Angle::fromRad(motion.fullSplineSamples.back().poses.back().orientation); + + base::Angle motionStartAngle = base::Angle::fromRad(motion.startTheta.getRadian()); + base::Angle motionEndAngle = base::Angle::fromRad(motion.endTheta.getRadian()); + + if (motion.type == Motion::MOV_FORWARD){ + BOOST_CHECK_CLOSE_FRACTION(splineStartAngle.getRad(), motionStartAngle.getRad(), 0.01); + BOOST_CHECK_CLOSE_FRACTION(splineEndAngle.getRad(), motionEndAngle.getRad(), 0.01); + } + + //Backward motions have a 180 degree angle diff between spline and motion + else if (motion.type == Motion::MOV_BACKWARD){ + BOOST_CHECK_CLOSE_FRACTION(std::abs((splineStartAngle - motionStartAngle).getRad()), 3.142, 0.01); + BOOST_CHECK_CLOSE_FRACTION(std::abs((splineEndAngle - motionEndAngle).getRad()), 3.142, 0.01); + } + + //Lateral motions have a 90 degree angle diff between spline and motion (robot maintains heading) + else if (motion.type == Motion::MOV_LATERAL){ + BOOST_CHECK_CLOSE_FRACTION(std::abs((splineStartAngle - motionStartAngle).getRad()), 1.57, 0.01); + BOOST_CHECK_CLOSE_FRACTION(std::abs((splineEndAngle - motionEndAngle).getRad()), 1.57, 0.01); + } + + const base::Pose2D& splineFinalPosition = motion.fullSplineSamples.back().poses.back(); + const base::Pose2D& travMapFinalPosition = motion.intermediateStepsTravMap.back().pose; + const base::Pose2D& obstMapFinalPosition = motion.intermediateStepsObstMap.back().pose; + + //Check if end position of full spline and sampled spline are same + BOOST_REQUIRE(splineFinalPosition.position.isApprox(travMapFinalPosition.position, 1e-6)); + BOOST_REQUIRE(splineFinalPosition.position.isApprox(obstMapFinalPosition.position, 1e-6)); + } + } + } + + delete motions; +} + +BOOST_AUTO_TEST_CASE(calculate_cost) { + + sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; + Mobility mobilityConfig; + + PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); + motions->computeMotions(splinePrimitiveConfig.gridSize, splinePrimitiveConfig.gridSize); + + //Start angle 0 degrees (First set of primitives from 16 sets) + DiscreteTheta theta(1,16); + std::vector motions_start = motions->getMotionForStartTheta(theta); + BOOST_REQUIRE(motions_start.size() > 0); + BOOST_CHECK_THROW(motions_start[0].calculateCost(0,0,0,0,0), std::runtime_error); + + //Translation + BOOST_CHECK_GT(motions_start[0].calculateCost(1,0,0.1,0.1,1), 0); + //Rotation + BOOST_CHECK_GT(motions_start[0].calculateCost(0,1,0.1,0.1,1), 0); + //Translation and rotation + BOOST_CHECK_GT(motions_start[0].calculateCost(1,1,0.1,0.1,1), 0); + + delete motions; +} \ No newline at end of file diff --git a/src/test/test_ugv_nav4d.cpp b/src/test/test_ugv_nav4d.cpp deleted file mode 100644 index c4c9a0d..0000000 --- a/src/test/test_ugv_nav4d.cpp +++ /dev/null @@ -1,274 +0,0 @@ -#define BOOST_TEST_MODULE PlannerTestModule -#include - -#include -#include - -#include "ugv_nav4d/DiscreteTheta.hpp" -#include "ugv_nav4d/Planner.hpp" -#include -#include - -#include -#include -#include - -using namespace ugv_nav4d; - -std::string filePath; - -class PlannerTest { -public: - PlannerTest(); - ~PlannerTest(); - void loadMlsMap(const std::string& path); - std::string getResult(const Planner::PLANNING_RESULT& result); - - Planner* planner; - maps::grid::MLSMapSloped mlsMap; - PlannerConfig plannerConfig; - Mobility mobility; - sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; - traversability_generator3d::TraversabilityConfig traversabilityConfig; - bool map_loaded = false; -}; - -PlannerTest::PlannerTest() { - splinePrimitiveConfig.gridSize = 0.3; - splinePrimitiveConfig.numAngles = 42; - splinePrimitiveConfig.numEndAngles = 21; - splinePrimitiveConfig.destinationCircleRadius = 10; - splinePrimitiveConfig.cellSkipFactor = 3; - splinePrimitiveConfig.splineOrder = 4.0; - - mobility.translationSpeed = 0.5; - mobility.rotationSpeed = 0.5; - mobility.minTurningRadius = 1; - mobility.spline_sampling_resolution = 0.05; - mobility.remove_goal_offset = true; - mobility.multiplierForward = 1; - mobility.multiplierBackward = 3; - mobility.multiplierPointTurn = 3; - mobility.multiplierLateral = 4; - mobility.multiplierForwardTurn = 2; - mobility.multiplierBackwardTurn = 4; - mobility.multiplierLateralCurve = 4; - mobility.searchRadius = 0.0; - mobility.searchProgressSteps = 0.1; - mobility.maxMotionCurveLength = 100; - - traversabilityConfig.maxStepHeight = 0.25; - traversabilityConfig.maxSlope = 0.45; - traversabilityConfig.inclineLimittingMinSlope = 0.2; - traversabilityConfig.inclineLimittingLimit = 0.1; - traversabilityConfig.costFunctionDist = 0.0; - traversabilityConfig.minTraversablePercentage = 0.4; - traversabilityConfig.robotHeight = 1.2; - traversabilityConfig.robotSizeX = 1.35; - traversabilityConfig.robotSizeY = 0.85; - traversabilityConfig.distToGround = 0.0; - traversabilityConfig.slopeMetricScale = 1.0; - traversabilityConfig.slopeMetric = traversability_generator3d::NONE; - traversabilityConfig.gridResolution = 0.3; - traversabilityConfig.initialPatchVariance = 0.0001; - traversabilityConfig.allowForwardDownhill = true; - traversabilityConfig.enableInclineLimitting = false; - - plannerConfig.searchUntilFirstSolution = false; - plannerConfig.initialEpsilon = 64; - plannerConfig.epsilonSteps = 2; - plannerConfig.numThreads = 8; - - loadMlsMap(filePath); -} - -PlannerTest::~PlannerTest() { - delete planner; -} - -void PlannerTest::loadMlsMap(const std::string& path) { - std::ifstream fileIn(path); - if (path.find(".ply") != std::string::npos) { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - pcl::PLYReader plyReader; - if (plyReader.read(path, *cloud) >= 0) { - pcl::PointXYZ mi, ma; - pcl::getMinMax3D(*cloud, mi, ma); - - Eigen::Affine3f pclTf = Eigen::Affine3f::Identity(); - pclTf.translation() << -mi.x, -mi.y, -mi.z; - pcl::transformPointCloud(*cloud, *cloud, pclTf); - - const double mls_res = 0.3; - const double size_x = ma.x; - const double size_y = ma.y; - - const maps::grid::Vector2ui numCells(size_x / mls_res + 1, size_y / mls_res + 1); - maps::grid::MLSConfig cfg; - cfg.gapSize = 0.1; - cfg.thickness = 0.1; - cfg.useColor = false; - mlsMap = maps::grid::MLSMapSloped(numCells, maps::grid::Vector2d(mls_res, mls_res), cfg); - mlsMap.mergePointCloud(*cloud, base::Transform3d::Identity()); - map_loaded = true; - return; - } - } - map_loaded = false; -} - -std::string PlannerTest::getResult(const Planner::PLANNING_RESULT& result) { - switch (result) { - case Planner::GOAL_INVALID: return "GOAL_INVALID"; - case Planner::START_INVALID: return "START_INVALID"; - case Planner::NO_SOLUTION: return "NO_SOLUTION"; - case Planner::NO_MAP: return "NO_MAP"; - case Planner::INTERNAL_ERROR: return "INTERNAL_ERROR"; - case Planner::FOUND_SOLUTION: return "FOUND_SOLUTION"; - default: return "ERROR unknown result state"; - } -} - -// Individual Tests -BOOST_FIXTURE_TEST_SUITE(PlannerTestSuite, PlannerTest) - -// Converted test case from GTest to Boost -BOOST_AUTO_TEST_CASE(check_planner_init_failure_wrong_grid_resolutions) { - traversabilityConfig.gridResolution = 0.4; - - try { - planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); - } catch (const std::exception& ex) { - std::cout << "Exception: \n" << ex.what() << "\n"; - BOOST_CHECK_NE(splinePrimitiveConfig.gridSize, traversabilityConfig.gridResolution); - } -} - -BOOST_AUTO_TEST_CASE(check_planner_init_success) { - planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); - BOOST_CHECK(planner != nullptr); -} - -BOOST_AUTO_TEST_CASE(check_planner_goal_invalid) { - BOOST_REQUIRE(map_loaded); // equivalent to EXPECT_EQ(map_loaded, true) - - planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); - BOOST_REQUIRE(planner != nullptr); - planner->updateMap(mlsMap); - - base::samples::RigidBodyState startState; - startState.position.x() = 2.3; - startState.position.y() = 1.2; - startState.position.z() = 0.0; - startState.orientation.w() = 1; - - base::samples::RigidBodyState endState; - endState.position.x() = 11.0; - endState.position.y() = 0.0; - endState.position.z() = 0.0; - endState.orientation.w() = 1; - - int maxTime = 5; - std::vector trajectory2D; - std::vector trajectory3D; - - std::cout << "Planning: \n"; - const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); - std::cout << "Planning Result: " << getResult(result) << std::endl; - BOOST_CHECK_EQUAL(result, Planner::GOAL_INVALID); -} - -BOOST_AUTO_TEST_CASE(check_planner_start_invalid) { - BOOST_REQUIRE(map_loaded); - - planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); - BOOST_REQUIRE(planner != nullptr); - planner->updateMap(mlsMap); - - base::samples::RigidBodyState startState; - startState.position.x() = 0.0; - startState.position.y() = 0.0; - startState.position.z() = 0.0; - startState.orientation.w() = 1; - - base::samples::RigidBodyState endState; - endState.position.x() = 1.0; - endState.position.y() = 0.0; - endState.position.z() = 0.0; - endState.orientation.w() = 1; - - int maxTime = 5; - std::vector trajectory2D; - std::vector trajectory3D; - - std::cout << "Planning: \n"; - const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); - std::cout << "Planning Result: " << getResult(result) << std::endl; - BOOST_CHECK_EQUAL(result, Planner::START_INVALID); -} - -BOOST_AUTO_TEST_CASE(check_planner_success) { - BOOST_REQUIRE(map_loaded); - - planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); - BOOST_REQUIRE(planner != nullptr); - planner->updateMap(mlsMap); - - base::samples::RigidBodyState startState; - startState.position.x() = 2.3; - startState.position.y() = 4.1; - startState.position.z() = 0.0; - startState.orientation.w() = 1; - - base::samples::RigidBodyState endState; - endState.position.x() = 6.1; - endState.position.y() = 4.2; - endState.position.z() = 0.0; - endState.orientation.w() = 1; - - int maxTime = 5; - std::vector trajectory2D; - std::vector trajectory3D; - - std::cout << "Planning: \n"; - const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); - std::cout << "Planning Result: " << getResult(result) << std::endl; - BOOST_CHECK_EQUAL(result, Planner::FOUND_SOLUTION); -} - -// DiscreteTheta test -BOOST_AUTO_TEST_CASE(check_discrete_theta_init) { - DiscreteTheta theta = DiscreteTheta(0, 16); - BOOST_CHECK_CLOSE(theta.getRadian(), 0, 0.001); - - theta = DiscreteTheta(1, 16); - BOOST_CHECK_EQUAL(theta.getNumAngles(), 16); - BOOST_CHECK_CLOSE(theta.getRadian(), 0.3926, 0.001); - - theta = DiscreteTheta(-1, 16); - BOOST_CHECK_EQUAL(theta.getTheta(), 15); - BOOST_CHECK_CLOSE(theta.getRadian(), 5.8904, 0.001); - - theta = DiscreteTheta(18, 16); - BOOST_CHECK_EQUAL(theta.getTheta(), 2); - - theta = DiscreteTheta(16, 16); - BOOST_CHECK_EQUAL(theta.getTheta(), 0); - - theta = DiscreteTheta(5.90, 16); - BOOST_CHECK_EQUAL(theta.getTheta(), 15); - - theta = DiscreteTheta(5.45, 16); - BOOST_CHECK_CLOSE(theta.getRadian(), 5.45, 0.1); - BOOST_CHECK_EQUAL(theta.getTheta(), 14); - - DiscreteTheta thetaA = DiscreteTheta(3, 16); - DiscreteTheta thetaB = DiscreteTheta(5, 16); - BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 2); - - thetaA = DiscreteTheta(2, 16); - thetaB = DiscreteTheta(14, 16); - BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 4); -} - -BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file