Add support for new PTGs with internalState in MRPT >=2.14.2

MRPT · Oct 5, 2024 · fa3e402 · fa3e402
1 parent 955af40
commit fa3e402
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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();
 }