Skip to content

Commit bc782f7

Browse files
jlblancocjlblancoc
committed
mrpt_nav: simplified code
1 parent 89ce03e commit bc782f7

File tree

8 files changed

+362
-202
lines changed

8 files changed

+362
-202
lines changed

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

Lines changed: 0 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -338,45 +338,6 @@ increasing risk/effort. Each item is prefixed with a priority tag:
338338
339339
---
340340
341-
#### 13.6.3 Naming consistency (remaining)
342-
343-
- **Config structs**: Holonomic classes use `TOptions`; navigator/optimizer/graphslam
344-
classes use `TParams`. Low-value cosmetic change across multiple modules — deferred.
345-
346-
---
347-
348-
#### 13.6.7 Decompose `CAbstractPTGBasedReactive`
349-
350-
**[P2]** This 467-line header (11 virtual methods, 6 nested structs,
351-
private members spanning PTG management, holonomic method management,
352-
logging, obstacle processing, velocity generation, and profiling) is the
353-
single largest maintenance burden. Proposed decomposition:
354-
355-
1. **Extract `PTGSetManager`** — owns the `vector<CParameterizedTrajectoryGenerator::Ptr>`,
356-
handles `initPTGs()`, `getPTG()`, `getPTG_count()`,
357-
collision-grid builds.
358-
2. **Extract `NavigationLogger`** — owns the `CLogFileRecord`,
359-
`m_critZoneLastLog`, log-file path management, `generateLogRecord()`.
360-
3. **Extract `VelocityFilter`** — owns `TSentVelCmd`, the speed-filter
361-
tau, and `filterVelocityCommand()`.
362-
4. Keep the main class as a thin orchestrator that wires the above
363-
together via dependency injection (constructor parameters or setters).
364-
365-
---
366-
367-
#### 13.6.8 Refactor long functions
368-
369-
**[P2]** Several functions exceed 200 lines and mix multiple levels of
370-
abstraction. Each should be broken into named helpers:
371-
372-
| Function | Lines | Suggested split |
373-
|----------|-------|-----------------|
374-
| `CLogFileRecord::serializeFrom()` | ~450 | Version-switch per struct field group (high backward-compat risk, deferred) |
375-
376-
---
377-
378-
---
379-
380341
### 13.7 `mrpt_maps` — Miscellaneous
381342
382343
- **`CObservationRotatingScan`**: `// TODO: populate organizedPoints?`

‎modules/mrpt_nav/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,8 @@ set(LIB_SOURCES
3232
# reactive/
3333
src/reactive/CAbstractNavigator.cpp
3434
src/reactive/CAbstractPTGBasedReactive.cpp
35+
src/reactive/NavigationLogger.cpp
36+
src/reactive/VelocityFilter.cpp
3537
src/reactive/CLogFileRecord.cpp
3638
src/reactive/CMultiObjectiveMotionOptimizerBase.cpp
3739
src/reactive/CMultiObjMotionOpt_Scalarization.cpp
@@ -83,6 +85,8 @@ set(LIB_PUBLIC_HEADERS
8385
# reactive/
8486
include/mrpt/nav/reactive/CAbstractNavigator.h
8587
include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h
88+
include/mrpt/nav/reactive/NavigationLogger.h
89+
include/mrpt/nav/reactive/VelocityFilter.h
8690
include/mrpt/nav/reactive/CLogFileRecord.h
8791
include/mrpt/nav/reactive/CMultiObjectiveMotionOptimizerBase.h
8892
include/mrpt/nav/reactive/CMultiObjMotionOpt_Scalarization.h

‎modules/mrpt_nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h‎

Lines changed: 20 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,9 @@
2121
#include <mrpt/nav/reactive/CLogFileRecord.h>
2222
#include <mrpt/nav/reactive/CMultiObjectiveMotionOptimizerBase.h>
2323
#include <mrpt/nav/reactive/CWaypointsNavigator.h>
24+
#include <mrpt/nav/reactive/NavigationLogger.h>
2425
#include <mrpt/nav/reactive/TCandidateMovementPTG.h>
26+
#include <mrpt/nav/reactive/VelocityFilter.h>
2527
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
2628
#include <mrpt/system/CTimeLogger.h>
2729
#include <mrpt/system/datetime.h>
@@ -152,18 +154,22 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
152154
* \note Log records are not prepared unless either "enableLogFile" is
153155
* enabled in the constructor or "enableLogFile()" has been called.
154156
*/
155-
void getLastLogRecord(CLogFileRecord& o);
157+
void getLastLogRecord(CLogFileRecord& o) { m_navLogger.getLastLogRecord(o); }
156158

157159
/** Enables keeping an internal registry of navigation logs that can be
158160
* queried with getLastLogRecord() */
159-
void enableKeepLogRecords(bool enable = true) { m_enableKeepLogRecords = enable; }
161+
void enableKeepLogRecords(bool enable = true) { m_navLogger.enableKeepLogRecords(enable); }
160162

161163
/** Enables/disables saving log files. */
162164
void enableLogFile(bool enable);
163165

164166
/** Changes the prefix for new log files. */
165-
void setLogFileDirectory(const std::string& sDir) { m_navlogfiles_dir = sDir; }
166-
std::string getLogFileDirectory() const { return m_navlogfiles_dir; }
167+
void setLogFileDirectory(const std::string& sDir) { m_navLogger.setLogFileDirectory(sDir); }
168+
[[nodiscard]] std::string getLogFileDirectory() const
169+
{
170+
return m_navLogger.getLogFileDirectory();
171+
}
172+
167173
struct TAbstractPTGNavigatorParams : public mrpt::config::CLoadableOptions
168174
{
169175
/** C++ class name of the holonomic navigation method to run in the
@@ -273,18 +279,12 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
273279
/** The holonomic navigation algorithm (one object per PTG, so internal
274280
* states are maintained) */
275281
std::vector<CAbstractHolonomicReactiveMethod::Ptr> m_holonomicMethod;
276-
std::unique_ptr<mrpt::io::CStream> m_logFile;
277-
/** The current log file stream, or nullptr if not being used */
278-
mrpt::io::CStream* m_prev_logfile{nullptr};
279-
/** See enableKeepLogRecords */
280-
bool m_enableKeepLogRecords{false};
281-
/** The last log */
282-
CLogFileRecord lastLogRecord;
283-
/** Last velocity commands */
284-
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd;
285-
286-
/** Critical zones */
287-
std::recursive_mutex m_critZoneLastLog;
282+
283+
/** Navigation log file manager */
284+
NavigationLogger m_navLogger;
285+
286+
/** Velocity command filtering and last-sent-command tracking */
287+
VelocityFilter m_velFilter;
288288

289289
/** Enables / disables the console debug output. */
290290
bool m_enableConsoleOutput;
@@ -368,9 +368,11 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
368368
const mrpt::system::TTimeStamp tim_start_iteration,
369369
const mrpt::nav::CHolonomicLogFileRecord::Ptr& hlfr);
370370

371-
/** Return the [0,1] velocity scale of raw PTG cmd_vel */
371+
/** Return the [0,1] velocity scale of raw PTG cmd_vel.
372+
* Delegates to VelocityFilter. */
372373
virtual double generate_vel_cmd(
373374
const TCandidateMovementPTG& in_movement, mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd);
375+
374376
void generateLogRecord(
375377
CLogFileRecord& newLogRec,
376378
const std::vector<mrpt::math::TPose2D>& relTargets,
@@ -432,7 +434,7 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
432434
TSentVelCmd() = default;
433435

434436
void reset() { *this = TSentVelCmd(); }
435-
bool isValid() const { return this->poseVel.timestamp != INVALID_TIMESTAMP; }
437+
[[nodiscard]] bool isValid() const { return this->poseVel.timestamp != INVALID_TIMESTAMP; }
436438
/** 0-based index of used PTG */
437439
int ptg_index = -1;
438440
/** Path index for selected PTG */
@@ -457,9 +459,6 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
457459
/** Delete m_holonomicMethod */
458460
void deleteHolonomicObjects();
459461

460-
/** Default: "./reactivenav.logs" */
461-
std::string m_navlogfiles_dir;
462-
463462
double m_expr_var_k, m_expr_var_k_target, m_expr_var_num_paths;
464463
/** A copy of last-iteration navparams, used to detect changes */
465464
std::unique_ptr<TNavigationParams> m_copy_prev_navParams;

‎modules/mrpt_nav/include/mrpt/nav/reactive/NavigationLogger.h‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
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+
#pragma once
15+
16+
#include <mrpt/io/CStream.h>
17+
#include <mrpt/nav/reactive/CLogFileRecord.h>
18+
#include <mrpt/serialization/CArchive.h>
19+
#include <mrpt/system/CTimeLogger.h>
20+
#include <mrpt/system/filesystem.h>
21+
22+
#include <memory>
23+
#include <mutex>
24+
#include <string>
25+
26+
namespace mrpt::nav
27+
{
28+
/** Manages navigation log file writing and in-memory log record keeping.
29+
*
30+
* Extracted from CAbstractPTGBasedReactive to reduce its complexity.
31+
* \ingroup nav_reactive
32+
*/
33+
class NavigationLogger
34+
{
35+
public:
36+
NavigationLogger() = default;
37+
38+
/** Enable/disable log file output.
39+
* \param enable If true, opens a new log file; if false, closes any open file.
40+
* \param logDir Directory for log files.
41+
* \param logger Optional COutputLogger for debug messages.
42+
*/
43+
void enableLogFile(bool enable, const std::string& logDir, mrpt::system::COutputLogger* logger);
44+
45+
/** Whether log file or in-memory records should be filled */
46+
[[nodiscard]] bool shouldFillLogRecord() const { return m_logFile || m_enableKeepLogRecords; }
47+
48+
/** Enables keeping an internal registry of navigation logs */
49+
void enableKeepLogRecords(bool enable = true) { m_enableKeepLogRecords = enable; }
50+
51+
/** Provides a copy of the last log record */
52+
void getLastLogRecord(CLogFileRecord& o);
53+
54+
/** Changes the prefix for new log files. */
55+
void setLogFileDirectory(const std::string& sDir) { m_navlogfiles_dir = sDir; }
56+
[[nodiscard]] std::string getLogFileDirectory() const { return m_navlogfiles_dir; }
57+
58+
/** Write the completed log record to file and/or store as last record */
59+
void writeLogRecord(const CLogFileRecord& rec, mrpt::system::CTimeLogger& timelogger);
60+
61+
/** Access to the raw log stream (for introductory blocks, etc.) */
62+
[[nodiscard]] mrpt::io::CStream* getLogStream() { return m_logFile.get(); }
63+
64+
/** Returns true if this is the first time we write to a new log file
65+
* (used for writing PTG introductory data) */
66+
[[nodiscard]] bool isNewLogFile() const { return m_logFile && m_logFile.get() != m_prev_logfile; }
67+
68+
/** Mark the current log file as "already introduced" */
69+
void markLogFileIntroduced() { m_prev_logfile = m_logFile.get(); }
70+
71+
/** Close log file and reset state */
72+
void close();
73+
74+
private:
75+
std::unique_ptr<mrpt::io::CStream> m_logFile;
76+
mrpt::io::CStream* m_prev_logfile{nullptr};
77+
bool m_enableKeepLogRecords{false};
78+
CLogFileRecord m_lastLogRecord;
79+
std::recursive_mutex m_critZoneLastLog;
80+
std::string m_navlogfiles_dir{"./reactivenav.logs"};
81+
};
82+
83+
} // namespace mrpt::nav

‎modules/mrpt_nav/include/mrpt/nav/reactive/VelocityFilter.h‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
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+
#pragma once
15+
16+
#include <mrpt/kinematics/CVehicleVelCmd.h>
17+
#include <mrpt/nav/reactive/TCandidateMovementPTG.h>
18+
#include <mrpt/system/CTimeLogger.h>
19+
20+
namespace mrpt::nav
21+
{
22+
/** Handles velocity command generation with speed limits and filtering.
23+
*
24+
* Extracted from CAbstractPTGBasedReactive to reduce its complexity.
25+
* \ingroup nav_reactive
26+
*/
27+
class VelocityFilter
28+
{
29+
public:
30+
/** Generates a velocity command from the selected candidate movement,
31+
* applying speed limits and filtering.
32+
* \return The [0,1] velocity scale of the raw PTG cmd_vel */
33+
double generateVelCmd(
34+
const TCandidateMovementPTG& in_movement,
35+
mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd,
36+
double speedfilter_tau,
37+
const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& robot_absolute_speed_limits,
38+
double meanExecutionPeriod,
39+
mrpt::system::CTimeLogger& timelogger);
40+
41+
/** Access to the last velocity command (for hysteresis comparisons) */
42+
[[nodiscard]] mrpt::kinematics::CVehicleVelCmd::Ptr getLastVelCmd() const
43+
{
44+
return m_last_vel_cmd;
45+
}
46+
47+
/** Reset the last velocity command state */
48+
void resetLastVelCmd() { m_last_vel_cmd.reset(); }
49+
50+
private:
51+
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd;
52+
};
53+
54+
} // namespace mrpt::nav

0 commit comments

Comments
 (0)