Skip to content

Commit 7b15a16

Browse files
jlblancocjlblancoc
committed
Progress with nav refactor
1 parent 0327219 commit 7b15a16

File tree

4 files changed

+331
-20
lines changed

4 files changed

+331
-20
lines changed

‎doc/source/doxygen-docs/port_mrpt3.md‎

Lines changed: 18 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -480,20 +480,24 @@ abstraction. Each should be broken into named helpers:
480480
481481
#### 13.6.10 Test coverage
482482
483-
**[P1]** Current coverage is approximately 25 %. The following classes
484-
have **zero dedicated unit tests** and should be prioritised:
485-
486-
- **Holonomic methods in isolation**: `CHolonomicVFF`, `CHolonomicND`,
487-
`CHolonomicFullEval` — currently only tested indirectly inside
488-
integration tests.
489-
- **Waypoint navigation**: `CWaypointsNavigator`, `TWaypoint`,
490-
`TWaypointSequence` — substantial state machine, no unit tests.
491-
- **Manual-sequence navigator**: `CNavigatorManualSequence`.
492-
- **RRT planners**: `PlannerRRT_SE2_TPS`, `PlannerRRT_common`.
493-
- **Multi-objective optimiser**: `CMultiObjectiveMotionOptimizerBase`,
494-
`CMultiObjMotionOpt_Scalarization`.
495-
- **Clearance diagram**: `ClearanceDiagram` — data structure used
496-
everywhere, zero tests.
483+
**[PARTIAL]** Added `tests/holonomic_unittest.cpp` covering:
484+
485+
- `CHolonomicVFF` in isolation (5 tests: heading accuracy × 3, speed
486+
bounds, slow-down near target, behaviour with blocked obstacles).
487+
- `CHolonomicND` in isolation (4 tests: heading accuracy × 2, speed
488+
bounds, detour when target direction blocked).
489+
- `CHolonomicFullEval` in isolation (5 tests: heading accuracy × 3, speed
490+
bounds, repeated calls / hysteresis stability). Also fixed two latent
491+
bugs: default `TOptions` had 7 factorWeights for `NUM_FACTORS=8`, and
492+
factor[7] (heading) dereferenced `ptg` without a null guard.
493+
- `ClearanceDiagram` (4 tests: construction, resize, clear, resize-twice).
494+
495+
Still zero dedicated tests for:
496+
497+
- `CWaypointsNavigator` / `TWaypointSequence` state machine.
498+
- `CNavigatorManualSequence`.
499+
- `PlannerRRT_SE2_TPS`, `PlannerRRT_common`.
500+
- `CMultiObjectiveMotionOptimizerBase`, `CMultiObjMotionOpt_Scalarization`.
497501
498502
---
499503

‎modules/mrpt_nav/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ set(LIB_SOURCES
5959
tests/PlannerSimple2D_unittest.cpp
6060
tests/PTGs_unittest.cpp
6161
tests/CLogFileRecord_unittest.cpp
62+
tests/holonomic_unittest.cpp
6263
tests/rnav_unittest.cpp
6364
)
6465

‎modules/mrpt_nav/src/holonomic/CHolonomicFullEval.cpp‎

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -276,10 +276,17 @@ void CHolonomicFullEval::evalSingleTarget(
276276

277277
// Factor [7]: Heading mismatch: 1.0=perfect phi aligment, 0.0=180deg error
278278
// -------------------------------------------------------------------------
279-
uint32_t ptgStep = 0;
280-
ptg->getPathStepForDist(i, d * ptg->getRefDistance(), ptgStep);
281-
const auto ptgPose = ptg->getPathPose(i, ptgStep);
282-
scores[7] = 1.0 - std::abs(mrpt::math::angDistance(target.phi, ptgPose.phi) / M_PI);
279+
if (ptg != nullptr)
280+
{
281+
uint32_t ptgStep = 0;
282+
ptg->getPathStepForDist(i, d * ptg->getRefDistance(), ptgStep);
283+
const auto ptgPose = ptg->getPathPose(i, ptgStep);
284+
scores[7] = 1.0 - std::abs(mrpt::math::angDistance(target.phi, ptgPose.phi) / M_PI);
285+
}
286+
else
287+
{
288+
scores[7] = 1.0; // neutral when no PTG heading info available
289+
}
283290

284291
// Save stats for debugging:
285292
for (size_t l = 0; l < NUM_FACTORS; l++) m_dirs_scores(i, l) = scores[l];
@@ -620,9 +627,9 @@ void CLogFileRecord_FullEval::serializeFrom(mrpt::serialization::CArchive& in, u
620627
---------------------------------------------------------------*/
621628
CHolonomicFullEval::TOptions::TOptions() :
622629
factorWeights{
623-
0.1, 0.5, 0.5, 0.01, 1, 1, 1
630+
0.1, 0.5, 0.5, 0.01, 1, 1, 1, 0.0
624631
},
625-
factorNormalizeOrNot{0, 0, 0, 0, 1, 0, 0},
632+
factorNormalizeOrNot{0, 0, 0, 0, 1, 0, 0, 0},
626633
PHASE_FACTORS{{1, 2}, {4}, {0, 2}},
627634
PHASE_THRESHOLDS{0.5, 0.6, 0.7}
628635

‎modules/mrpt_nav/tests/holonomic_unittest.cpp‎

Lines changed: 299 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,299 @@
1+
/* _
2+
| | Mobile Robot Programming Toolkit (MRPT)
3+
_ __ ___ _ __ _ __ | |_
4+
| '_ ` _ \| '__| '_ \| __| https://www.mrpt.org/
5+
| | | | | | | | |_) | |_
6+
|_| |_| |_|_| | .__/ \__| https://github.com/MRPT/mrpt/
7+
| |
8+
|_|
9+
10+
Copyright (c) 2005-2026, Individual contributors, see AUTHORS file
11+
See: https://www.mrpt.org/Authors - All rights reserved.
12+
SPDX-License-Identifier: BSD-3-Clause
13+
*/
14+
15+
/** Unit tests for holonomic navigation methods (CHolonomicVFF, CHolonomicND,
16+
* CHolonomicFullEval) and ClearanceDiagram, exercised in isolation without
17+
* a full PTG or reactive navigation system.
18+
*/
19+
20+
#include <gtest/gtest.h>
21+
#include <mrpt/nav/holonomic/CHolonomicFullEval.h>
22+
#include <mrpt/nav/holonomic/CHolonomicND.h>
23+
#include <mrpt/nav/holonomic/CHolonomicVFF.h>
24+
#include <mrpt/nav/holonomic/ClearanceDiagram.h>
25+
26+
#include <cmath>
27+
28+
using mrpt::nav::CAbstractHolonomicReactiveMethod;
29+
30+
// ---------------------------------------------------------------------------
31+
// Helper: build a NavInput with N directions, all free (obs=1.0), and a
32+
// single target at angle `target_angle_rad` with distance `target_dist`.
33+
// ---------------------------------------------------------------------------
34+
static CAbstractHolonomicReactiveMethod::NavInput makeNavInput(
35+
size_t nDirs, double target_angle_rad, double target_dist = 0.5)
36+
{
37+
CAbstractHolonomicReactiveMethod::NavInput ni;
38+
ni.obstacles.assign(nDirs, 1.0); // all free
39+
ni.maxObstacleDist = 1.0;
40+
ni.maxRobotSpeed = 1.0;
41+
ni.targets.emplace_back(
42+
target_dist * std::cos(target_angle_rad), target_dist * std::sin(target_angle_rad),
43+
0.0 /*phi*/);
44+
return ni;
45+
}
46+
47+
// Wrap angle to [-pi, pi]
48+
static double wrapAngle(double a)
49+
{
50+
while (a > M_PI) a -= 2 * M_PI;
51+
while (a < -M_PI) a += 2 * M_PI;
52+
return a;
53+
}
54+
55+
// ============================================================================
56+
// CHolonomicVFF tests
57+
// ============================================================================
58+
class HolonomicVFFTest : public ::testing::Test
59+
{
60+
protected:
61+
mrpt::nav::CHolonomicVFF vff;
62+
};
63+
64+
TEST_F(HolonomicVFFTest, navigate_toward_target_ahead)
65+
{
66+
constexpr double target_angle = 0.0; // directly forward
67+
auto ni = makeNavInput(100, target_angle);
68+
CAbstractHolonomicReactiveMethod::NavOutput no;
69+
vff.navigate(ni, no);
70+
71+
EXPECT_GT(no.desiredSpeed, 0.0);
72+
EXPECT_LT(no.desiredSpeed, 1.01);
73+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.3 /*rad*/);
74+
}
75+
76+
TEST_F(HolonomicVFFTest, navigate_toward_target_left)
77+
{
78+
constexpr double target_angle = M_PI / 2.0; // 90 deg left
79+
auto ni = makeNavInput(100, target_angle);
80+
CAbstractHolonomicReactiveMethod::NavOutput no;
81+
vff.navigate(ni, no);
82+
83+
EXPECT_GT(no.desiredSpeed, 0.0);
84+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.3 /*rad*/);
85+
}
86+
87+
TEST_F(HolonomicVFFTest, navigate_toward_target_right)
88+
{
89+
constexpr double target_angle = -M_PI / 4.0; // 45 deg right
90+
auto ni = makeNavInput(100, target_angle);
91+
CAbstractHolonomicReactiveMethod::NavOutput no;
92+
vff.navigate(ni, no);
93+
94+
EXPECT_GT(no.desiredSpeed, 0.0);
95+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.3 /*rad*/);
96+
}
97+
98+
TEST_F(HolonomicVFFTest, speed_within_bounds)
99+
{
100+
auto ni = makeNavInput(100, 0.0, 0.5);
101+
CAbstractHolonomicReactiveMethod::NavOutput no;
102+
vff.navigate(ni, no);
103+
104+
EXPECT_GE(no.desiredSpeed, 0.0);
105+
EXPECT_LE(no.desiredSpeed, ni.maxRobotSpeed + 1e-6);
106+
}
107+
108+
TEST_F(HolonomicVFFTest, slow_down_near_target)
109+
{
110+
// target very close → speed should be lower than for a distant target
111+
auto ni_far = makeNavInput(100, 0.0, 0.5);
112+
auto ni_near = makeNavInput(100, 0.0, 0.01); // almost at target
113+
114+
CAbstractHolonomicReactiveMethod::NavOutput no_far, no_near;
115+
vff.navigate(ni_far, no_far);
116+
vff.navigate(ni_near, no_near);
117+
118+
EXPECT_LE(no_near.desiredSpeed, no_far.desiredSpeed + 1e-6);
119+
}
120+
121+
TEST_F(HolonomicVFFTest, all_obstacles_present_reduces_speed)
122+
{
123+
// Fill all directions with close obstacles except the target direction
124+
CAbstractHolonomicReactiveMethod::NavInput ni;
125+
ni.obstacles.assign(100, 0.05); // everything very close
126+
ni.obstacles[50] = 1.0; // only straight ahead is free
127+
ni.maxObstacleDist = 1.0;
128+
ni.maxRobotSpeed = 1.0;
129+
ni.targets.emplace_back(0.3, 0.0, 0.0); // target ahead
130+
131+
CAbstractHolonomicReactiveMethod::NavOutput no;
132+
vff.navigate(ni, no);
133+
// Should not crash; output is well-defined
134+
EXPECT_GE(no.desiredSpeed, 0.0);
135+
EXPECT_LE(no.desiredSpeed, ni.maxRobotSpeed + 1e-6);
136+
}
137+
138+
// ============================================================================
139+
// CHolonomicND tests
140+
// ============================================================================
141+
class HolonomicNDTest : public ::testing::Test
142+
{
143+
protected:
144+
mrpt::nav::CHolonomicND nd;
145+
};
146+
147+
TEST_F(HolonomicNDTest, navigate_toward_target_ahead)
148+
{
149+
constexpr double target_angle = 0.0;
150+
auto ni = makeNavInput(121, target_angle);
151+
CAbstractHolonomicReactiveMethod::NavOutput no;
152+
nd.navigate(ni, no);
153+
154+
EXPECT_GT(no.desiredSpeed, 0.0);
155+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.35 /*rad*/);
156+
}
157+
158+
TEST_F(HolonomicNDTest, navigate_toward_target_left)
159+
{
160+
constexpr double target_angle = M_PI / 2.0;
161+
auto ni = makeNavInput(121, target_angle);
162+
CAbstractHolonomicReactiveMethod::NavOutput no;
163+
nd.navigate(ni, no);
164+
165+
EXPECT_GT(no.desiredSpeed, 0.0);
166+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.35 /*rad*/);
167+
}
168+
169+
TEST_F(HolonomicNDTest, speed_within_bounds)
170+
{
171+
auto ni = makeNavInput(121, 0.0);
172+
CAbstractHolonomicReactiveMethod::NavOutput no;
173+
nd.navigate(ni, no);
174+
175+
EXPECT_GE(no.desiredSpeed, 0.0);
176+
EXPECT_LE(no.desiredSpeed, ni.maxRobotSpeed + 1e-6);
177+
}
178+
179+
TEST_F(HolonomicNDTest, blocked_target_direction_detours)
180+
{
181+
// Target is at angle 0 but that direction is blocked; the robot should
182+
// pick a nearby detour rather than charging into the obstacle.
183+
auto ni = makeNavInput(121, 0.0);
184+
// Block a swath around angle=0 (indices near 60 out of 121)
185+
for (int k = 55; k <= 66; k++) ni.obstacles[k] = 0.05;
186+
187+
CAbstractHolonomicReactiveMethod::NavOutput no;
188+
nd.navigate(ni, no);
189+
190+
EXPECT_GE(no.desiredSpeed, 0.0);
191+
EXPECT_LE(no.desiredSpeed, ni.maxRobotSpeed + 1e-6);
192+
}
193+
194+
// ============================================================================
195+
// CHolonomicFullEval tests
196+
// ============================================================================
197+
class HolonomicFullEvalTest : public ::testing::Test
198+
{
199+
protected:
200+
mrpt::nav::CHolonomicFullEval fe;
201+
};
202+
203+
TEST_F(HolonomicFullEvalTest, navigate_toward_target_ahead)
204+
{
205+
// nDirs must be > 3 (asserted inside navigate())
206+
constexpr double target_angle = 0.0;
207+
auto ni = makeNavInput(121, target_angle);
208+
CAbstractHolonomicReactiveMethod::NavOutput no;
209+
fe.navigate(ni, no);
210+
211+
EXPECT_GT(no.desiredSpeed, 0.0);
212+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.35 /*rad*/);
213+
}
214+
215+
TEST_F(HolonomicFullEvalTest, navigate_toward_target_left)
216+
{
217+
constexpr double target_angle = M_PI / 2.0;
218+
auto ni = makeNavInput(121, target_angle);
219+
CAbstractHolonomicReactiveMethod::NavOutput no;
220+
fe.navigate(ni, no);
221+
222+
EXPECT_GT(no.desiredSpeed, 0.0);
223+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.35 /*rad*/);
224+
}
225+
226+
TEST_F(HolonomicFullEvalTest, navigate_toward_target_diagonal)
227+
{
228+
constexpr double target_angle = M_PI / 4.0; // 45 deg
229+
auto ni = makeNavInput(121, target_angle);
230+
CAbstractHolonomicReactiveMethod::NavOutput no;
231+
fe.navigate(ni, no);
232+
233+
EXPECT_GT(no.desiredSpeed, 0.0);
234+
EXPECT_LT(std::abs(wrapAngle(no.desiredDirection - target_angle)), 0.35 /*rad*/);
235+
}
236+
237+
TEST_F(HolonomicFullEvalTest, speed_within_bounds)
238+
{
239+
auto ni = makeNavInput(121, 0.0);
240+
CAbstractHolonomicReactiveMethod::NavOutput no;
241+
fe.navigate(ni, no);
242+
243+
EXPECT_GE(no.desiredSpeed, 0.0);
244+
EXPECT_LE(no.desiredSpeed, ni.maxRobotSpeed + 1e-6);
245+
}
246+
247+
TEST_F(HolonomicFullEvalTest, repeated_calls_do_not_crash)
248+
{
249+
// Hysteresis state is maintained between calls; verify no crash across
250+
// several consecutive navigate() invocations.
251+
for (int iter = 0; iter < 5; iter++)
252+
{
253+
const double angle = (iter % 2 == 0) ? 0.0 : M_PI / 4.0;
254+
auto ni = makeNavInput(121, angle);
255+
CAbstractHolonomicReactiveMethod::NavOutput no;
256+
fe.navigate(ni, no);
257+
EXPECT_GE(no.desiredSpeed, 0.0);
258+
EXPECT_LE(no.desiredSpeed, ni.maxRobotSpeed + 1e-6);
259+
}
260+
}
261+
262+
// ============================================================================
263+
// ClearanceDiagram tests
264+
// ============================================================================
265+
TEST(ClearanceDiagram, default_constructed_is_empty)
266+
{
267+
mrpt::nav::ClearanceDiagram cd;
268+
EXPECT_TRUE(cd.empty());
269+
}
270+
271+
TEST(ClearanceDiagram, resize_sets_path_counts)
272+
{
273+
mrpt::nav::ClearanceDiagram cd;
274+
cd.resize(100, 10);
275+
276+
EXPECT_FALSE(cd.empty());
277+
EXPECT_EQ(cd.get_actual_num_paths(), 100u);
278+
EXPECT_EQ(cd.get_decimated_num_paths(), 10u);
279+
}
280+
281+
TEST(ClearanceDiagram, clear_resets_to_empty)
282+
{
283+
mrpt::nav::ClearanceDiagram cd;
284+
cd.resize(50, 5);
285+
ASSERT_FALSE(cd.empty());
286+
287+
cd.clear();
288+
EXPECT_TRUE(cd.empty());
289+
}
290+
291+
TEST(ClearanceDiagram, resize_twice_updates_counts)
292+
{
293+
mrpt::nav::ClearanceDiagram cd;
294+
cd.resize(100, 10);
295+
cd.resize(200, 20);
296+
297+
EXPECT_EQ(cd.get_actual_num_paths(), 200u);
298+
EXPECT_EQ(cd.get_decimated_num_paths(), 20u);
299+
}

0 commit comments

Comments
 (0)