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