nav: update and modernize API

Author: jlblancoc

apps/mrpt_apps_gui/ReactiveNav3D-Demo/ReactiveNav3D_demo.h

@@ -473,18 +473,14 @@ class CMyReactInterface : public mrpt::nav::CRobot2NavInterfaceForSimulator_Diff
   gui::CDisplayWindow3D window;
   Scene::Ptr scene;
 
-  bool getCurrentPoseAndSpeeds(
-      mrpt::math::TPose2D& curPose,
-      mrpt::math::TTwist2D& curVel,
-      mrpt::system::TTimeStamp& timestamp,
-      mrpt::math::TPose2D& odomPose,
-      std::string& pose_frame_id) override
+  std::optional<CurrentPoseAndSpeeds> getCurrentPoseAndSpeeds() override
   {
-    curPose = robotSim.getCurrentGTPose();
-    curVel = robotSim.getCurrentGTVel();
-    timestamp = mrpt::Clock::now();
-    odomPose = robotSim.getCurrentOdometricPose();
-    return true;
+    return CurrentPoseAndSpeeds{
+        .pose = robotSim.getCurrentGTPose(),
+        .velGlobal = robotSim.getCurrentGTVel(),
+        .timestamp = mrpt::Clock::now(),
+        .odometry = robotSim.getCurrentOdometricPose(),
+    };
   }
 
   bool senseObstacles(

doc/source/doxygen-docs/port_mrpt3.md

@@ -338,90 +338,38 @@ increasing risk/effort.  Each item is prefixed with a priority tag:
 
 ---
 
-#### 13.6.1 Enum modernisation
-
-**[DONE]** Converted all plain `enum` to `enum class`, with redundant prefixes removed from values:
-
-| Old | New |
-|-----|-----|
-| `CAbstractNavigator::TState` — `IDLE`, `NAVIGATING`, `SUSPENDED`, `NAV_ERROR` | `enum class TState` — same values (`NAV_ERROR` kept to avoid clash with Windows `ERROR` macro) |
-| `CAbstractNavigator::TErrorCode` — `ERR_NONE`, `ERR_EMERGENCY_STOP`, `ERR_CANNOT_REACH_TARGET`, `ERR_OTHER` | `enum class TErrorCode` — `NONE`, `EMERGENCY_STOP`, `CANNOT_REACH_TARGET`, `OTHER` |
-| `CHolonomicND::TSituations` — `SITUATION_TARGET_DIRECTLY`, `SITUATION_SMALL_GAP`, `SITUATION_WIDE_GAP`, `SITUATION_NO_WAY_FOUND` | `enum class TSituations` — `TARGET_DIRECTLY`, `SMALL_GAP`, `WIDE_GAP`, `NO_WAY_FOUND` |
-| `PTG_collision_behavior_t` — `COLL_BEH_BACK_AWAY`, `COLL_BEH_STOP` | `enum class PTGCollisionBehavior` — `BACK_AWAY`, `STOP` |
-
----
-
-#### 13.6.2 `[[nodiscard]]` annotations
-
-**[DONE]** Added `[[nodiscard]]` to:
-
-- `CAbstractNavigator`: `getCurrentState()`, `getErrorReason()`,
-  `getFrameTF()`, `isRethrowNavExceptionsEnabled()`, `getDelaysTimeLogger()`
-- `CParameterizedTrajectoryGenerator`: `getDescription()`, `supportVelCmdNOP()`,
-  `supportSpeedAtTarget()`, `isInitialized()`, `getAlphaValuesCount()`,
-  `getPathCount()`, `getRefDistance()`, `getScorePriority()`,
-  `getClearanceStepCount()`, `getClearanceDecimatedPaths()`, `getPathStepCount()`,
-  `getPathPose()`, `getPathDist()`, `getPathStepDuration()`, `getMaxLinVel()`,
-  `getMaxAngVel()`, `getMaxRobotRadius()`, `isPointInsideRobotShape()`
-- `CAbstractHolonomicReactiveMethod`: `getAssociatedPTG()`
-- `CWaypointsNavigator`: `getWaypointsAccessGuard()`
+#### 13.6.2 `[[nodiscard]]` annotations (remaining)
 
 Still pending: `CAbstractPTGBasedReactive` getters, holonomic `TOptions` getters.
 
 ---
 
-#### 13.6.3 Naming consistency
-
-**[PARTIAL]** Done:
-- `CPTG_DiffDrive_CC/CCS/alpha`: replaced `os::sprintf` + `(int)K` casts with
-  `mrpt::format()` + `static_cast<int>(K)`.
+#### 13.6.3 Naming consistency (remaining)
 
-Remaining:
-- **Config structs**: Holonomic classes use `TOptions`; navigator classes use `TParams`.
-  Pick one suffix uniformly.
-- **STEP-numbered methods**: `STEP1_InitPTGs()` … `STEP8_GenerateLogRecord()` —
-  rename to descriptive names.
-- **`impl_` prefix**: Standardise virtual hook naming convention.
+- **Config structs**: Holonomic classes use `TOptions`; navigator/optimizer/graphslam
+  classes use `TParams`. Low-value cosmetic change across multiple modules — deferred.
 
 ---
 
-#### 13.6.4 Thread-safety fixes
-
-**[DONE]**:
-- `CAbstractPTGBasedReactive::preDestructor()`: replaced bare
-  `m_nav_cs.lock(); m_nav_cs.unlock()` with `{ std::scoped_lock lck(m_nav_cs); }`.
-- `CWaypointsNavigator`: replaced `beginWaypointsAccess()` / `endWaypointsAccess()`
-  with `getWaypointsAccessGuard()` returning a `WaypointsAccessGuard` RAII object
-  that holds `std::unique_lock<std::recursive_mutex>` and exposes `waypoints()`.
+#### 13.6.4 Thread-safety fixes (remaining)
 
-Remaining: global mutable `OUTPUT_DEBUG_PATH_PREFIX` / `COLLISION_BEHAVIOR` statics
+Global mutable `OUTPUT_DEBUG_PATH_PREFIX` / `COLLISION_BEHAVIOR` statics
 have no synchronisation — consider moving to per-instance fields or guarding with mutex.
 
 ---
 
-#### 13.6.5 Return-value modernisation (output-reference cleanup)
-
-**[TODO — P1]** Key candidates:
+#### 13.6.5 Return-value modernisation (remaining)
 
 | Method | Current | Proposed |
 |--------|---------|----------|
 | `CRobot2NavInterface::getCurrentPoseAndSpeeds()` | 4 output `&` params | Return `struct PoseAndSpeeds { TPose2D pose; TTwist2D vel; … };` |
-| `CAbstractHolonomicReactiveMethod::navigate()` | `NavOutput&` | Return `NavOutput` by value |
-| `CMultiObjectiveMotionOptimizerBase::decide()` | `int&` best index | Return `std::optional<size_t>` (nullopt = no good motion) |
 | `CParameterizedTrajectoryGenerator::inverseMap_WS2TP()` | `int& k, double& d` output | Return `std::optional<std::pair<uint16_t,double>>` |
 
 ---
 
-#### 13.6.6 Raw-pointer cleanup
-
-**[DONE — documented]**: Both `m_associatedPTG` in `CAbstractHolonomicReactiveMethod`
-and `TCandidateMovementPTG::PTG` are non-owning observer raw pointers. Both are now
-documented explicitly as such, with lifetime contract stated. Since `nullptr` is a valid
-state and the PTG lifetime is guaranteed by the owning reactive system, a raw pointer
-is the correct tool; `std::weak_ptr` would require the entire PTG ownership chain to
-be changed to shared ownership.
+#### 13.6.6 Raw-pointer cleanup (remaining)
 
-Remaining: `CAbstractPTGBasedReactive::getHoloMethod()` returns a raw pointer;
+`CAbstractPTGBasedReactive::getHoloMethod()` returns a raw pointer;
 consider returning `CAbstractHolonomicReactiveMethod&` (with assert) for non-nullable
 use sites.
 
@@ -435,10 +383,10 @@ logging, obstacle processing, velocity generation, and profiling) is the
 single largest maintenance burden.  Proposed decomposition:
 
 1. **Extract `PTGSetManager`** — owns the `vector<CParameterizedTrajectoryGenerator::Ptr>`,
-   handles `STEP1_InitPTGs()`, `getPTG()`, `getPTG_count()`,
+   handles `initPTGs()`, `getPTG()`, `getPTG_count()`,
    collision-grid builds.
 2. **Extract `NavigationLogger`** — owns the `CLogFileRecord`,
-   `m_critZoneLastLog`, log-file path management, `STEP8_GenerateLogRecord()`.
+   `m_critZoneLastLog`, log-file path management, `generateLogRecord()`.
 3. **Extract `VelocityFilter`** — owns `TSentVelCmd`, the speed-filter
    tau, and `filterVelocityCommand()`.
 4. Keep the main class as a thin orchestrator that wires the above
@@ -462,37 +410,9 @@ abstraction.  Each should be broken into named helpers:
 
 ---
 
-#### 13.6.9 Magic numbers → named constants
-
-**[DONE]**
-- `CHolonomicVFF.cpp`: `1e6` → `MAX_FORCE_CAP`, `20.0` → `OBSTACLE_WEIGHT_FACTOR`,
-  `6.0` → `OBSTACLE_NEARNESS_THRESHOLD`.
-- `CHolonomicND.cpp`: `0.02` → `DIRECT_PATH_SECTOR_FRACTION`,
-  `1.05` → `DIRECT_PATH_DIST_MARGIN`, `0.95` → `DIRECT_PATH_RANGE_FRACTION`,
-  `0.01` → `MIN_TARGET_DIST`.
-- `CHolonomicFullEval.cpp` (`evalSingleTarget`): `1.02` → `OBSTACLE_PAST_TARGET_MARGIN`,
-  `0.95` → `TARGET_DIST_SAFETY_FRACTION`, `1.05` → `TARGET_CLEARANCE_MARGIN`,
-  `1.01` → `SQRT_DOMAIN_EPS`, `0.1` → `CLEARANCE_WINDOW_HALF_FRACTION`,
-  `0.99` → `FREE_SPACE_THRESHOLD`, `4.0` → `SECTOR_DIST_WEIGHT`,
-  `0.1` → `BLOCKED_DIR_SCORE_PENALTY`.
-
----
-
-#### 13.6.10 Test coverage
+#### 13.6.10 Test coverage (remaining)
 
-**[PARTIAL]** Added `tests/holonomic_unittest.cpp` covering:
-
-- `CHolonomicVFF` in isolation (5 tests: heading accuracy × 3, speed
-  bounds, slow-down near target, behaviour with blocked obstacles).
-- `CHolonomicND` in isolation (4 tests: heading accuracy × 2, speed
-  bounds, detour when target direction blocked).
-- `CHolonomicFullEval` in isolation (5 tests: heading accuracy × 3, speed
-  bounds, repeated calls / hysteresis stability). Also fixed two latent
-  bugs: default `TOptions` had 7 factorWeights for `NUM_FACTORS=8`, and
-  factor[7] (heading) dereferenced `ptg` without a null guard.
-- `ClearanceDiagram` (4 tests: construction, resize, clear, resize-twice).
-
-Still zero dedicated tests for:
+Zero dedicated unit tests for:
 
 - `CWaypointsNavigator` / `TWaypointSequence` state machine.
 - `CNavigatorManualSequence`.
@@ -501,28 +421,6 @@ Still zero dedicated tests for:
 
 ---
 
-#### 13.6.11 Documentation
-
-**[DONE]** `lib_mrpt_nav.md` expanded to a full reference including:
-navigator state machine, waypoint sequencing (RAII guard API), TP-Space
-reactive core, holonomic methods table, PTG class table, robot interface
-guide, and path planning overview.
-
----
-
-#### 13.6.12 Remaining code-quality items
-
-**[DONE]** Quick wins completed:
-
-- `MRPT_TODO("Optimize getNearestNode() with KD-tree!")` in
-  `PlannerRRT_SE2_TPS.cpp` — converted to plain `// TODO:` comment.
-- `static size_t SAVE_LOG_SOLVE_COUNT` — kept (used to name debug log
-  files across solve() calls, guarded by `params.save_3d_log_freq > 0`).
-- `CLogFileRecord::serializeFrom()` raw `new` — replaced with
-  `std::make_shared` (three sites).
-- C-style casts `(int)K` — replaced with `static_cast<int>(K)` in
-  `CPTG_DiffDrive_CC.cpp`, `CPTG_DiffDrive_CCS.cpp`, `CPTG_DiffDrive_alpha.cpp`.
-
 ### 13.7 `mrpt_maps` — Miscellaneous
 
 - **`CObservationRotatingScan`**: `// TODO: populate organizedPoints?`

modules/mrpt_nav/include/mrpt/nav/holonomic/CAbstractHolonomicReactiveMethod.h

@@ -87,7 +87,7 @@ class CAbstractHolonomicReactiveMethod : public mrpt::serialization::CSerializab
 
   /** Invokes the holonomic navigation algorithm itself. See the description
    * of the input/output structures for details on each parameter. */
-  virtual void navigate(const NavInput& ni, NavOutput& no) = 0;
+  [[nodiscard]] virtual NavOutput navigate(const NavInput& ni) = 0;
 
   /** ctor */
   CAbstractHolonomicReactiveMethod(const std::string& defaultCfgSectionName);

modules/mrpt_nav/include/mrpt/nav/holonomic/CHolonomicFullEval.h

@@ -45,7 +45,7 @@ class CHolonomicFullEval : public CAbstractHolonomicReactiveMethod
   CHolonomicFullEval(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
 
   // See base class docs
-  void navigate(const NavInput& ni, NavOutput& no) override;
+  [[nodiscard]] NavOutput navigate(const NavInput& ni) override;
 
   void initialize(const mrpt::config::CConfigFileBase& INI_FILE) override;  // See base class docs
   void saveConfigFile(mrpt::config::CConfigFileBase& c) const override;     // See base class docs

modules/mrpt_nav/include/mrpt/nav/holonomic/CHolonomicND.h

@@ -64,7 +64,7 @@ class CHolonomicND : public CAbstractHolonomicReactiveMethod
   CHolonomicND(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
 
   // See base class docs
-  void navigate(const NavInput& ni, NavOutput& no) override;
+  [[nodiscard]] NavOutput navigate(const NavInput& ni) override;
 
   /** The structure used to store a detected gap in obstacles. */
   struct TGap

modules/mrpt_nav/include/mrpt/nav/holonomic/CHolonomicVFF.h

@@ -59,7 +59,7 @@ class CHolonomicVFF : public CAbstractHolonomicReactiveMethod
   CHolonomicVFF(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
 
   // See base class docs
-  void navigate(const NavInput& ni, NavOutput& no) override;
+  [[nodiscard]] NavOutput navigate(const NavInput& ni) override;
 
   void initialize(const mrpt::config::CConfigFileBase& INI_FILE) override;  // See base class docs
   void saveConfigFile(mrpt::config::CConfigFileBase& c) const override;     // See base class docs

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

@@ -134,7 +134,7 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
 
   /** Must be called for loading collision grids, or the first navigation
    * command may last a long time to be executed.
-   * Internally, it just calls STEP1_CollisionGridsBuilder().
+   * Internally, it just calls initPTGs().
    */
   void initialize() override;
 
@@ -312,18 +312,18 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
 
   // Steps for the reactive navigation sytem.
   // ----------------------------------------------------------------------------
-  virtual void STEP1_InitPTGs() = 0;
+  virtual void initPTGs() = 0;
 
   /** Return false on any fatal error */
   virtual bool implementSenseObstacles(mrpt::system::TTimeStamp& obs_timestamp) = 0;
-  bool STEP2_SenseObstacles();
+  bool senseObstacles();
 
   /** Builds TP-Obstacles from Workspace obstacles for the given PTG.
    * "out_TPObstacles" is already initialized to the proper length and
    * maximum collision-free distance for each "k" trajectory index.
    * Distances are in "pseudo-meters". They will be normalized automatically
    * to [0,1] upon return. */
-  virtual void STEP3_WSpaceToTPSpace(
+  virtual void transformToTPSpace(
       const size_t ptg_idx,
       std::vector<double>& out_TPObstacles,
       mrpt::nav::ClearanceDiagram& out_clearance,
@@ -370,7 +370,7 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator
   /** Return the [0,1] velocity scale of raw PTG cmd_vel */
   virtual double generate_vel_cmd(
       const TCandidateMovementPTG& in_movement, mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd);
-  void STEP8_GenerateLogRecord(
+  void generateLogRecord(
       CLogFileRecord& newLogRec,
       const std::vector<mrpt::math::TPose2D>& relTargets,
       int nSelectedPTG,

modules/mrpt_nav/include/mrpt/nav/reactive/CMultiObjMotionOpt_Scalarization.h

@@ -61,7 +61,7 @@ class CMultiObjMotionOpt_Scalarization : public mrpt::nav::CMultiObjectiveMotion
   std::map<std::string, double> m_expr_scalar_vars;
 
   // This virtual method is called by decide().
-  int impl_decide(
+  std::optional<size_t> impl_decide(
       const std::vector<mrpt::nav::TCandidateMovementPTG>& movs, TResultInfo& extra_info) override;
 };
 }  // namespace mrpt::nav

modules/mrpt_nav/include/mrpt/nav/reactive/CMultiObjectiveMotionOptimizerBase.h

@@ -19,6 +19,8 @@
 #include <mrpt/nav/reactive/TCandidateMovementPTG.h>
 #include <mrpt/rtti/CObject.h>
 
+#include <optional>
+
 namespace mrpt::nav
 {
 /** Virtual base class for multi-objective motion choosers, as used for reactive
@@ -50,9 +52,10 @@ class CMultiObjectiveMotionOptimizerBase : public mrpt::rtti::CObject
 
   /** The main entry point for the class: returns the 0-based index of the
    * best of the N motion candidates in `movs`.
-   * If no valid one is found, `-1` will be returned.
+   * If no valid one is found, `std::nullopt` will be returned.
    */
-  int decide(const std::vector<mrpt::nav::TCandidateMovementPTG>& movs, TResultInfo& extra_info);
+  [[nodiscard]] std::optional<size_t> decide(
+      const std::vector<mrpt::nav::TCandidateMovementPTG>& movs, TResultInfo& extra_info);
 
   virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c) = 0;
   virtual void saveConfigFile(mrpt::config::CConfigFileBase& c) const = 0;
@@ -97,7 +100,7 @@ class CMultiObjectiveMotionOptimizerBase : public mrpt::rtti::CObject
 
  private:
   // This virtual method is called by decide().
-  virtual int impl_decide(
+  virtual std::optional<size_t> impl_decide(
       const std::vector<mrpt::nav::TCandidateMovementPTG>& movs, TResultInfo& extra_info) = 0;
 
   TParamsBase& m_params_base;

modules/mrpt_nav/include/mrpt/nav/reactive/CReactiveNavigationSystem.h

@@ -119,7 +119,7 @@ class CReactiveNavigationSystem : public CAbstractPTGBasedReactive
 
   // Steps for the reactive navigation sytem.
   // ----------------------------------------------------------------------------
-  void STEP1_InitPTGs() override;
+  void initPTGs() override;
 
   // See docs in parent class
   bool implementSenseObstacles(mrpt::system::TTimeStamp& obs_timestamp) override;
@@ -142,7 +142,7 @@ class CReactiveNavigationSystem : public CAbstractPTGBasedReactive
   /** Obstacle points, before filtering (if filtering is enabled). */
   mrpt::maps::CSimplePointsMap m_WS_Obstacles_original;
   // See docs in parent class
-  void STEP3_WSpaceToTPSpace(
+  void transformToTPSpace(
       size_t ptg_idx,
       std::vector<double>& out_TPObstacles,
       mrpt::nav::ClearanceDiagram& out_clearance,