From fa3e40229a7f02dee2c97eb45e861356b487505d Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Sat, 5 Oct 2024 08:17:52 +0200
Subject: [PATCH] Add support for new PTGs with internalState in MRPT >=2.14.2

---
 mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h |  5 +++++
 mrpt_path_planning/src/algos/TPS_Astar.cpp            |  5 +++++
 mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp       | 11 +++++++++++
 3 files changed, 21 insertions(+)

diff --git a/mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h b/mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h
index d0dece3..3a4d52f 100644
--- a/mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h
+++ b/mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h
@@ -10,6 +10,7 @@
 #include <mpp/data/basic_types.h>
 #include <mpp/data/ptg_t.h>
 #include <mrpt/graphs/TNodeID.h>
+#include <mrpt/version.h>
 
 #include <cstdint>
 
@@ -50,6 +51,10 @@ struct MoveEdgeSE2_TPS
 
     ptg_t::TNavDynamicState getPTGDynState() const;
 
+#if MRPT_VERSION >= 0x20e02  // >=2.14.2
+    mrpt::containers::yaml ptgInternalState;
+#endif
+
     /** Subsampled path, in coordinates relative to "stateFrom", stored here
      *  for rendering purposes, to avoid having to re-seed the PTG
      *  with the initial velocity state while visualization,
diff --git a/mrpt_path_planning/src/algos/TPS_Astar.cpp b/mrpt_path_planning/src/algos/TPS_Astar.cpp
index 3343550..d1ce47f 100644
--- a/mrpt_path_planning/src/algos/TPS_Astar.cpp
+++ b/mrpt_path_planning/src/algos/TPS_Astar.cpp
@@ -14,6 +14,7 @@
 #include <mpp/ptgs/SpeedTrimmablePTG.h>
 #include <mrpt/math/wrap2pi.h>
 #include <mrpt/opengl/COpenGLScene.h>
+#include <mrpt/version.h>
 
 #include <iostream>
 #include <unordered_set>
@@ -327,6 +328,10 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
             newEdge.ptgFinalRelativeGoal =
                 in.stateGoal.asSE2KinState().pose - current.state.pose;
 
+#if MRPT_VERSION >= 0x20e02  // >=2.14.2
+            newEdge.ptgInternalState =
+                ptg.getCurrentNavDynamicState().internalState;
+#endif
             newEdge.stateFrom = current.state;
             newEdge.stateTo   = x_i;
 
diff --git a/mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp b/mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp
index 32cc7ad..93da42c 100644
--- a/mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp
+++ b/mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp
@@ -5,6 +5,7 @@
  * ------------------------------------------------------------------------- */
 
 #include <mpp/data/MoveEdgeSE2_TPS.h>
+#include <mrpt/version.h>
 
 #include <sstream>
 
@@ -21,6 +22,10 @@ ptg_t::TNavDynamicState MoveEdgeSE2_TPS::getPTGDynState() const
     newDyn.relTarget      = ptgFinalRelativeGoal;
     newDyn.targetRelSpeed = ptgFinalGoalRelSpeed;
 
+#if MRPT_VERSION >= 0x20e02  // >=2.14.2
+    newDyn.internalState = ptgInternalState;
+#endif
+
     return newDyn;
 }
 
@@ -64,5 +69,11 @@ std::string MoveEdgeSE2_TPS::asString() const
        << estimatedExecTime << "}\n"
        << "  interpolatedPathSize: " << interpolatedPath.size() << "\n";
 
+#if MRPT_VERSION >= 0x20e02  // >=2.14.2
+    ss << ", "
+          "ptgInternalState: "
+       << ptgInternalState << "\n";
+#endif
+
     return ss.str();
 }