Merge pull request #1325 from MRPT/feature/ptg-internal-state

Feature/ptg internal state

apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp

@@ -1377,13 +1377,14 @@ void reactive_navigator_demoframe::simulateOneStep(double time_step)
       if (ptg)
       {
         // Draw path:
+        const auto& ipp = lfr.infoPerPTG[lfr.nSelectedPTG];
+
         const int selected_k =
-            is_NOP_op ? lfr.ptg_last_k_NOP
-                      : ptg->alpha2index(lfr.infoPerPTG[lfr.nSelectedPTG].desiredDirection);
+            is_NOP_op ? lfr.ptg_last_k_NOP : ptg->alpha2index(ipp.desiredDirection);
         float max_dist = ptg->getRefDistance();
         gl_robot_ptg_prediction->clear();
 
-        ptg->updateNavDynamicState(is_NOP_op ? lfr.ptg_last_navDynState : lfr.navDynState);
+        ptg->updateNavDynamicState(is_NOP_op ? ipp.lastDynState : ipp.dynState);
 
         ptg->renderPathAsSimpleLine(selected_k, *gl_robot_ptg_prediction, 0.10, max_dist);
         gl_robot_ptg_prediction->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00));

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(

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

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); }

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"

libs/nav/src/reactive/CLogFileRecord.cpp

@@ -21,43 +21,40 @@ using namespace mrpt::nav;
 
 IMPLEMENTS_SERIALIZABLE(CLogFileRecord, CSerializable, mrpt::nav)
 
-uint8_t CLogFileRecord::serializeGetVersion() const { return 27; }
+uint8_t CLogFileRecord::serializeGetVersion() const { return 28; }
 void CLogFileRecord::serializeTo(mrpt::serialization::CArchive& out) const
 {
-  uint32_t i, n;
-
   // Version 0 ---------
-  n = infoPerPTG.size();
-  out << n;
-  for (i = 0; i < n; i++)
+  out.WriteAs<uint32_t>(infoPerPTG.size());
+  for (const auto& ipp : infoPerPTG)
   {
-    out << infoPerPTG[i].PTG_desc.c_str();
+    out << ipp.PTG_desc.c_str();
 
-    uint32_t m = infoPerPTG[i].TP_Obstacles.size();
+    uint32_t m = ipp.TP_Obstacles.size();
     out << m;
     if (m)
-      out.WriteBuffer(
-          (const void*)&(*infoPerPTG[i].TP_Obstacles.begin()),
-          m * sizeof(infoPerPTG[i].TP_Obstacles[0]));
-
-    out << infoPerPTG[i].TP_Targets;  // v8: CPoint2D -> TPoint2D. v26: vector
-    out << infoPerPTG[i].TP_Robot;    // v17
-    out << infoPerPTG[i].timeForTPObsTransformation
-        << infoPerPTG[i].timeForHolonomicMethod;  // made double in v12
-    out << infoPerPTG[i].desiredDirection << infoPerPTG[i].desiredSpeed
-        << infoPerPTG[i].evaluation;  // made double in v12
+      out.WriteBuffer((const void*)&(*ipp.TP_Obstacles.begin()), m * sizeof(ipp.TP_Obstacles[0]));
+
+    out << ipp.TP_Targets;  // v8: CPoint2D -> TPoint2D. v26: vector
+    out << ipp.TP_Robot;    // v17
+    out << ipp.timeForTPObsTransformation << ipp.timeForHolonomicMethod;  // made double in v12
+    out << ipp.desiredDirection << ipp.desiredSpeed << ipp.evaluation;    // made double in v12
     // removed in v23: out << evaluation_org << evaluation_priority; //
     // added in v21
-    out << infoPerPTG[i].HLFR;
+    out << ipp.HLFR;
 
     // Version 9: Removed security distances. Added optional field with
     // PTG info.
-    const bool there_is_ptg_data = infoPerPTG[i].ptg ? true : false;
+    const bool there_is_ptg_data = ipp.ptg ? true : false;
     out << there_is_ptg_data;
-    if (there_is_ptg_data) out << infoPerPTG[i].ptg;
+    if (there_is_ptg_data) out << ipp.ptg;
 
-    // Was: out << infoPerPTG[i].clearance.raw_clearances; // v19
-    infoPerPTG[i].clearance.writeToStream(out);  // v25
+    // Was: out << ipp.clearance.raw_clearances; // v19
+    ipp.clearance.writeToStream(out);  // v25
+
+    // v28
+    ipp.dynState.writeToStream(out);
+    ipp.lastDynState.writeToStream(out);
   }
   out << nSelectedPTG << WS_Obstacles;
   out << WS_Obstacles_original;  // v20
@@ -72,15 +69,15 @@ void CLogFileRecord::serializeTo(mrpt::serialization::CArchive& out) const
       << rel_pose_PTG_origin_wrt_sense_NOP;  // v24: CPose2D->TPose2D
 
   // Removed: v24. out << ptg_last_curRobotVelLocal; // v17
-  ptg_last_navDynState.writeToStream(out);  // v24
+  // Removed: v28 ptg_last_navDynState.writeToStream(out);  // v24
 
   if (ptg_index_NOP < 0) out << cmd_vel /*v10*/ << cmd_vel_original;  // v15
 
   // Previous values: REMOVED IN VERSION #6
-  n = robotShape_x.size();
-  out << n;
-  if (n)
+  out.WriteAs<uint32_t>(robotShape_x.size());
+  if (!robotShape_x.empty())
   {
+    const auto n = robotShape_x.size();
     out.WriteBuffer((const void*)&(*robotShape_x.begin()), n * sizeof(robotShape_x[0]));
     out.WriteBuffer((const void*)&(*robotShape_y.begin()), n * sizeof(robotShape_y[0]));
   }
@@ -90,11 +87,11 @@ void CLogFileRecord::serializeTo(mrpt::serialization::CArchive& out) const
   // out << estimatedExecutionPeriod; // removed v13
 
   // Version 3 ----------
-  for (i = 0; i < infoPerPTG.size(); i++)
+  for (const auto& ipp : infoPerPTG)
   {
     // v22: this is now a TParameters
     // (mrpt 2.1.0) Directly a std::map<>
-    out << infoPerPTG[i].evalFactors;
+    out << ipp.evalFactors;
   }
 
   out << nPTGs;  // v4
@@ -109,7 +106,7 @@ void CLogFileRecord::serializeTo(mrpt::serialization::CArchive& out) const
   // v15: cmd_vel converted from std::vector<double> into CSerializable
   out << additional_debug_msgs;  // v18
 
-  navDynState.writeToStream(out);  // v24
+  // Removed v28. navDynState.writeToStream(out);  // v24.
 
   out << visuals;  // v27
 }
@@ -146,6 +143,7 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve
     case 25:
     case 26:
     case 27:
+    case 28:
     {
       // Version 0 --------------
       uint32_t i, n;
@@ -239,6 +237,12 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve
         {
           ipp.clearance.clear();
         }
+
+        if (version >= 28)
+        {
+          ipp.dynState.readFromStream(in);
+          ipp.lastDynState.readFromStream(in);
+        }
       }
 
       in >> nSelectedPTG >> WS_Obstacles;
@@ -306,11 +310,15 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve
       }
       if (version >= 17 && version < 24)
       {
-        in >> ptg_last_navDynState.curVelLocal;
+        mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds;
+        in >> ds.curVelLocal;
+        for (auto& ipp : infoPerPTG) ipp.dynState = ds;
       }
-      if (version >= 24)
+      if (version >= 24 && version < 28)
       {
-        ptg_last_navDynState.readFromStream(in);
+        mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds;
+        ds.readFromStream(in);
+        for (auto& ipp : infoPerPTG) ipp.dynState = ds;
       }
 
       if (version >= 10)
@@ -518,13 +526,19 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve
       else
         additional_debug_msgs.clear();
 
-      if (version >= 24)
-        navDynState.readFromStream(in);
-      else
+      if (version < 28)
       {
-        navDynState = CParameterizedTrajectoryGenerator::TNavDynamicState();
-        navDynState.curVelLocal = cur_vel_local;
-        if (!WS_targets_relative.empty()) navDynState.relTarget = WS_targets_relative[0];
+        CParameterizedTrajectoryGenerator::TNavDynamicState ds;
+        if (version >= 24)
+        {
+          ds.readFromStream(in);
+        }
+        else
+        {
+          ds.curVelLocal = cur_vel_local;
+          if (!WS_targets_relative.empty()) ds.relTarget = WS_targets_relative[0];
+        }
+        for (auto& ipp : infoPerPTG) ipp.dynState = ds;
       }
 
       if (version >= 27)

libs/nav/src/tpspace/CParameterizedTrajectoryGenerator.cpp

@@ -555,10 +555,14 @@ bool CParameterizedTrajectoryGenerator::TNavDynamicState::operator==(
 void mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::writeToStream(
     mrpt::serialization::CArchive& out) const
 {
-  const uint8_t version = 0;
+  const uint8_t version = 1;
   out << version;
   // Data:
   out << curVelLocal << relTarget << targetRelSpeed;
+  // internalState:
+  std::stringstream ss;
+  internalState.printAsYAML(ss);
+  out << ss.str();
 }
 
 void mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::readFromStream(
@@ -569,7 +573,21 @@ void mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::readFromStr
   switch (version)
   {
     case 0:
+    case 1:
       in >> curVelLocal >> relTarget >> targetRelSpeed;
+      if (version >= 1)
+      {
+        std::string s;
+        in >> s;
+        if (s.empty())
+          internalState.clear();
+        else
+          internalState = mrpt::containers::yaml::FromText(s);
+      }
+      else
+      {
+        internalState.clear();
+      }
       break;
     default:
       MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);

python/src/mrpt/maps/CVoxelMapBase.cpp

@@ -39,7 +39,7 @@
 
 void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string const &namespace_) > &M)
 {
-	{ // mrpt::maps::CVoxelMapBase file:mrpt/maps/CVoxelMapBase.h line:42
+	{ // mrpt::maps::CVoxelMapBase file:mrpt/maps/CVoxelMapBase.h line:44
 		pybind11::class_<mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>, std::shared_ptr<mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t", "");
 		cl.def("assign", (class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccupancy> & (mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>::*)(const class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccupancy> &)) &mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>::operator=, "C++: mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>::operator=(const class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccupancy> &) --> class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccupancy> &", pybind11::return_value_policy::automatic, pybind11::arg("o"));
 		cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>::*)() const) &mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>::asString, "C++: mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccupancy>::asString() const --> std::string");
@@ -70,7 +70,7 @@ void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string
 		cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic);
 		cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg(""));
 	}
-	{ // mrpt::maps::CVoxelMapBase file:mrpt/maps/CVoxelMapBase.h line:42
+	{ // mrpt::maps::CVoxelMapBase file:mrpt/maps/CVoxelMapBase.h line:44
 		pybind11::class_<mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>, std::shared_ptr<mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t", "");
 		cl.def("assign", (class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccRGB> & (mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>::*)(const class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccRGB> &)) &mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>::operator=, "C++: mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>::operator=(const class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccRGB> &) --> class mrpt::maps::CVoxelMapBase<struct mrpt::maps::VoxelNodeOccRGB> &", pybind11::return_value_policy::automatic, pybind11::arg("o"));
 		cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>::*)() const) &mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>::asString, "C++: mrpt::maps::CVoxelMapBase<mrpt::maps::VoxelNodeOccRGB>::asString() const --> std::string");