Add generic internalState to PTGs

MRPT · Oct 3, 2024 · 8abda5f · 8abda5f
1 parent 229073f
commit 8abda5f
Show file tree

Hide file tree

Showing 6 changed files with 158 additions and 285 deletions.
diff --git a/apps/navlog-viewer/navlog-viewer-ui.cpp b/apps/navlog-viewer/navlog-viewer-ui.cpp
@@ -808,13 +808,14 @@ void NavlogViewerApp::updateVisualization()
         {
           if (!ptg->isInitialized()) ptg->initialize();
 
+          auto& ipp = log.infoPerPTG[sel_ptg_idx];
+
           // Set instantaneous dyn state:
-          ptg->updateNavDynamicState(is_NOP_cmd ? log.ptg_last_navDynState : log.navDynState);
+          ptg->updateNavDynamicState(is_NOP_cmd ? ipp.lastDynState : ipp.dynState);
 
           // Draw path:
           const int selected_k =
-              log.ptg_index_NOP < 0 ? ptg->alpha2index(log.infoPerPTG[sel_ptg_idx].desiredDirection)
-                                    : log.ptg_last_k_NOP;
+              log.ptg_index_NOP < 0 ? ptg->alpha2index(ipp.desiredDirection) : log.ptg_last_k_NOP;
           float max_dist = ptg->getRefDistance();
           ptg->add_robotShape_to_setOfLines(*gl_path);
 
@@ -916,8 +917,13 @@ void NavlogViewerApp::updateVisualization()
         {
           if (!ptg->isInitialized()) ptg->initialize();
 
+          const bool this_NOP_cmd =
+              m_manualPickPTGIdx == static_cast<int>(m_logdata_ptg_paths.size() - 1);
+
+          auto& ipp = log.infoPerPTG[m_manualPickPTGIdx];
+
           // Set instantaneous dyn state:
-          ptg->updateNavDynamicState(is_NOP_cmd ? log.ptg_last_navDynState : log.navDynState);
+          ptg->updateNavDynamicState(this_NOP_cmd ? ipp.lastDynState : ipp.dynState);
 
           // Draw path:
           const int selected_k = m_manualPickTrajectoryIdx;
@@ -1137,13 +1143,16 @@ void NavlogViewerApp::updateVisualization()
 
   if (m_cbShowAllDebugFields->checked())
   {
-    ADD_WIN_TEXTMSG(format(
-                        "navDynState: curVelLocal=%s relTarget=%s "
-                        "targetRelSpeed=%.02f",
-                        log.navDynState.curVelLocal.asString().c_str(),
-                        log.navDynState.relTarget.asString().c_str(),
-                        log.navDynState.targetRelSpeed)
-                        .c_str());
+    if (!log.infoPerPTG.empty())
+    {
+      ADD_WIN_TEXTMSG(format(
+                          "navDynState: curVelLocal=%s relTarget=%s "
+                          "targetRelSpeed=%.02f",
+                          log.infoPerPTG.front().dynState.curVelLocal.asString().c_str(),
+                          log.infoPerPTG.front().dynState.relTarget.asString().c_str(),
+                          log.infoPerPTG.front().dynState.targetRelSpeed)
+                          .c_str());
+    }
 
     for (const auto& e : log.values)
       ADD_WIN_TEXTMSG(format(

diff --git a/libs/nav/include/mrpt/nav/reactive/CLogFileRecord.h b/libs/nav/include/mrpt/nav/reactive/CLogFileRecord.h
@@ -65,18 +65,19 @@ class CLogFileRecord : public mrpt::serialization::CSerializable
     mrpt::nav::CParameterizedTrajectoryGenerator::Ptr ptg;
     /** Clearance for each path */
     mrpt::nav::ClearanceDiagram clearance;
+    /** Stored for each PTG to cope with custom internal states */
+    mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState dynState, lastDynState;
   };
 
-  mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState navDynState;
   /** The number of PTGS: */
-  uint32_t nPTGs{0};
+  uint32_t nPTGs = 0;
 
   /** The info for each applied PTG: must contain "nPTGs * nSecDistances"
    * elements */
   std::vector<TInfoPerPTG> infoPerPTG;
 
   /** The selected PTG. */
-  int32_t nSelectedPTG{-1};
+  int32_t nSelectedPTG = -1;
 
   /** Known values:
    *	- "executionTime": The total computation time, excluding sensing.
@@ -136,7 +137,6 @@ class CLogFileRecord : public mrpt::serialization::CSerializable
   uint16_t ptg_last_k_NOP{0};
   mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP{0, 0, 0},
       rel_pose_PTG_origin_wrt_sense_NOP{0, 0, 0};
-  mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ptg_last_navDynState;
 
   /** Additional visual entities that the source application wants to show
    * in the navlog viewer UI. All objects will be placed relative to the

diff --git a/libs/nav/include/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h b/libs/nav/include/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h
@@ -10,6 +10,7 @@
 
 #include <mrpt/config/CConfigFileBase.h>
 #include <mrpt/config/CLoadableOptions.h>
+#include <mrpt/containers/yaml.h>
 #include <mrpt/core/round.h>
 #include <mrpt/kinematics/CVehicleVelCmd.h>
 #include <mrpt/math/CPolygon.h>
@@ -182,6 +183,9 @@ class CParameterizedTrajectoryGenerator :
     /** Desired relative speed [0,1] at target. Default=0 */
     double targetRelSpeed{0};
 
+    /** Derived-class specific internal state variables */
+    mrpt::containers::yaml internalState;
+
     TNavDynamicState() = default;
     bool operator==(const TNavDynamicState& o) const;
     inline bool operator!=(const TNavDynamicState& o) const { return !(*this == o); }

diff --git a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp
@@ -22,8 +22,6 @@
 #include <mrpt/serialization/CArchive.h>
 #include <mrpt/system/filesystem.h>
 
-#include <array>
-#include <iomanip>
 #include <limits>
 
 using namespace mrpt;
@@ -470,8 +468,6 @@ void CAbstractPTGBasedReactive::performNavigationStep()
     ptg_dynState.relTarget = relTargets[0];
     ptg_dynState.targetRelSpeed = m_navigationParams->target.targetDesiredRelSpeed;
 
-    newLogRec.navDynState = ptg_dynState;
-
     {
       mrpt::system::CTimeLoggerEntry tle2(
           m_navProfiler,
@@ -916,7 +912,6 @@ void CAbstractPTGBasedReactive::STEP8_GenerateLogRecord(
   newLogRec.rel_pose_PTG_origin_wrt_sense_NOP = rel_pose_PTG_origin_wrt_sense_NOP;
   newLogRec.ptg_index_NOP = best_is_NOP_cmdvel ? m_lastSentVelCmd.ptg_index : -1;
   newLogRec.ptg_last_k_NOP = m_lastSentVelCmd.ptg_alpha_index;
-  newLogRec.ptg_last_navDynState = m_lastSentVelCmd.ptg_dynState;
 
   m_timelogger.leave("navigationStep.populate_log_info");
 
@@ -1606,7 +1601,14 @@ void CAbstractPTGBasedReactive::build_movement_candidate(
           this_is_PTG_continuation ? m_lastSentVelCmd.colfreedist_move_k : .0;
 
       //  SAVE LOG
-      newLogRec.infoPerPTG[idx_in_log_infoPerPTGs].evalFactors = cm.props;
+      auto& ipp = newLogRec.infoPerPTG[idx_in_log_infoPerPTGs];
+
+      ipp.evalFactors = cm.props;
+
+      if (m_lastSentVelCmd.ptg_index == static_cast<int>(idx_in_log_infoPerPTGs))
+        ipp.lastDynState = m_lastSentVelCmd.ptg_dynState;
+
+      ipp.dynState = ptg->getCurrentNavDynamicState();
     }
 
   }  // end "valid_TP"