Skip to content

Commit 0eef6dd

Browse files
author
Mulo01
committed
Add unit tests for environmentxyztheta
1 parent 743b6e2 commit 0eef6dd

File tree

3 files changed

+375
-323
lines changed

3 files changed

+375
-323
lines changed

‎src/test/CMakeLists.txt

+19-3
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,22 @@
1-
find_package(GTest REQUIRED)
21
add_executable(test_ugv_nav4d test_ugv_nav4d.cpp)
2+
add_executable(test_EnvironmentXYZTheta test_EnvironmentXYZTheta.cpp)
3+
4+
target_include_directories(test_ugv_nav4d PUBLIC ${Boost_INCLUDE_DIR})
5+
target_include_directories(test_EnvironmentXYZTheta PUBLIC ${Boost_INCLUDE_DIR})
6+
37

48
if (ROCK_QT_VERSION_4)
5-
target_link_libraries(test_ugv_nav4d GTest::GTest ugv_nav4d)
9+
target_link_libraries(test_ugv_nav4d PRIVATE
10+
ugv_nav4d)
11+
target_link_libraries(test_EnvironmentXYZTheta PRIVATE
12+
ugv_nav4d)
613
endif()
714

815
if (ROCK_QT_VERSION_5)
9-
target_link_libraries(test_ugv_nav4d GTest::GTest ugv_nav4d-qt5)
16+
target_link_libraries(test_ugv_nav4d PRIVATE
17+
ugv_nav4d-qt5)
18+
target_link_libraries(test_EnvironmentXYZTheta PRIVATE
19+
ugv_nav4d-qt5)
1020
endif()
1121

1222
# Install the binaries
@@ -15,3 +25,9 @@ install(TARGETS test_ugv_nav4d EXPORT test_ugv_nav4d-targets
1525
LIBRARY DESTINATION lib
1626
RUNTIME DESTINATION bin
1727
)
28+
29+
install(TARGETS test_EnvironmentXYZTheta EXPORT test_EnvironmentXYZTheta-targets
30+
ARCHIVE DESTINATION lib
31+
LIBRARY DESTINATION lib
32+
RUNTIME DESTINATION bin
33+
)

‎src/test/test_EnvironmentXYZTheta.cpp

+130
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
#define BOOST_TEST_MODULE EnvironmentXYZThetaTestModule
2+
3+
#include <fstream>
4+
#include <cstdlib>
5+
6+
#include "ugv_nav4d/EnvironmentXYZTheta.hpp"
7+
#include "ugv_nav4d/Mobility.hpp"
8+
#include <sbpl_spline_primitives/SplinePrimitivesConfig.hpp>
9+
#include <traversability_generator3d/TraversabilityConfig.hpp>
10+
11+
#include <maps/grid/MLSMap.hpp>
12+
13+
#include <pcl/io/ply_io.h>
14+
#include <pcl/common/common.h>
15+
#include <pcl/common/transforms.h>
16+
17+
#include <boost/test/included/unit_test.hpp>
18+
#include <boost/archive/binary_oarchive.hpp>
19+
#include <boost/archive/binary_iarchive.hpp>
20+
21+
using namespace ugv_nav4d;
22+
23+
std::string mlsBinFile;
24+
25+
// Define EnvironmentXYZThetaTest fixture for Boost Test
26+
struct EnvironmentXYZThetaTest {
27+
EnvironmentXYZThetaTest();
28+
~EnvironmentXYZThetaTest();
29+
30+
void loadMlsMap();
31+
32+
typedef EnvironmentXYZTheta::MLGrid MLSBase;
33+
34+
EnvironmentXYZTheta* environment;
35+
maps::grid::MLSMapSloped mlsMap;
36+
Mobility mobility;
37+
sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig;
38+
traversability_generator3d::TraversabilityConfig traversabilityConfig;
39+
bool map_loaded = false;
40+
};
41+
42+
EnvironmentXYZThetaTest::EnvironmentXYZThetaTest() {
43+
std::cout << "Called SetUp()" << std::endl;
44+
45+
splinePrimitiveConfig.gridSize = 0.3;
46+
splinePrimitiveConfig.numAngles = 16;
47+
splinePrimitiveConfig.numEndAngles = 8;
48+
splinePrimitiveConfig.destinationCircleRadius = 6;
49+
splinePrimitiveConfig.cellSkipFactor = 0.1;
50+
splinePrimitiveConfig.splineOrder = 4.0;
51+
52+
mobility.translationSpeed = 0.5;
53+
mobility.rotationSpeed = 0.5;
54+
mobility.minTurningRadius = 1;
55+
mobility.spline_sampling_resolution = 0.05;
56+
mobility.remove_goal_offset = true;
57+
mobility.multiplierForward = 1;
58+
mobility.multiplierBackward = 3;
59+
mobility.multiplierPointTurn = 3;
60+
mobility.multiplierLateral = 4;
61+
mobility.multiplierForwardTurn = 2;
62+
mobility.multiplierBackwardTurn = 4;
63+
mobility.multiplierLateralCurve = 4;
64+
mobility.searchRadius = 1.0;
65+
mobility.searchProgressSteps = 0.1;
66+
mobility.maxMotionCurveLength = 100;
67+
68+
traversabilityConfig.maxStepHeight = 0.25;
69+
traversabilityConfig.maxSlope = 0.45;
70+
traversabilityConfig.inclineLimittingMinSlope = 0.2;
71+
traversabilityConfig.inclineLimittingLimit = 0.1;
72+
traversabilityConfig.costFunctionDist = 0.0;
73+
traversabilityConfig.minTraversablePercentage = 0.4;
74+
traversabilityConfig.robotHeight = 1.2;
75+
traversabilityConfig.robotSizeX = 1.35;
76+
traversabilityConfig.robotSizeY = 0.85;
77+
traversabilityConfig.distToGround = 0.0;
78+
traversabilityConfig.slopeMetricScale = 1.0;
79+
traversabilityConfig.slopeMetric = traversability_generator3d::NONE;
80+
traversabilityConfig.gridResolution = 0.3;
81+
traversabilityConfig.initialPatchVariance = 0.0001;
82+
traversabilityConfig.allowForwardDownhill = true;
83+
traversabilityConfig.enableInclineLimitting = false;
84+
85+
loadMlsMap();
86+
}
87+
88+
EnvironmentXYZThetaTest::~EnvironmentXYZThetaTest(){
89+
std::cout << "Called TearDown()" << std::endl;
90+
delete environment;
91+
}
92+
93+
void EnvironmentXYZThetaTest::loadMlsMap() {
94+
std::string filename;
95+
96+
if (mlsBinFile.empty()) {
97+
filename = "cave_circuit_mls.bin";
98+
} else {
99+
filename = mlsBinFile;
100+
}
101+
102+
std::ifstream file(filename, std::ios::binary);
103+
if (!file.is_open()) {
104+
map_loaded = false;
105+
return;
106+
}
107+
108+
try {
109+
boost::archive::binary_iarchive ia(file);
110+
ia >> mlsMap; // Deserialize into mlsMap
111+
} catch (const std::exception& e) {
112+
map_loaded = false;
113+
}
114+
map_loaded = true;
115+
}
116+
117+
// Define the test suite and test cases
118+
BOOST_FIXTURE_TEST_SUITE(EnvironmentXYZThetaTestSuite, EnvironmentXYZThetaTest)
119+
120+
BOOST_AUTO_TEST_CASE(check_planner_init_failure_wrong_grid_resolutions) {
121+
std::shared_ptr<MLSBase> mlsPtr = std::make_shared<MLSBase>(mlsMap);
122+
try {
123+
environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility);
124+
} catch (const std::exception& ex) {
125+
std::cout << "Exception: \n" << ex.what() << "\n";
126+
BOOST_CHECK_NE(splinePrimitiveConfig.gridSize, traversabilityConfig.gridResolution);
127+
}
128+
}
129+
130+
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)