From 8abda5fcdf0b43c4af4bfc8ef74ffc8efbff6055 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 4 Oct 2024 01:16:58 +0200 Subject: [PATCH 1/4] Add generic internalState to PTGs --- apps/navlog-viewer/navlog-viewer-ui.cpp | 31 +- .../mrpt/nav/reactive/CLogFileRecord.h | 8 +- .../CParameterizedTrajectoryGenerator.h | 4 + .../reactive/CAbstractPTGBasedReactive.cpp | 14 +- libs/nav/src/reactive/CLogFileRecord.cpp | 366 +++++------------- .../CParameterizedTrajectoryGenerator.cpp | 20 +- 6 files changed, 158 insertions(+), 285 deletions(-) diff --git a/apps/navlog-viewer/navlog-viewer-ui.cpp b/apps/navlog-viewer/navlog-viewer-ui.cpp index ff9837479f..cc0dbe60f5 100644 --- 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(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 index cf4c09c9ee..81c14923d8 100644 --- 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 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 index f7d4a19ea0..b8184d059c 100644 --- a/libs/nav/include/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h +++ b/libs/nav/include/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -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 index 3876c9f99e..aabe248712 100644 --- a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp +++ b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp @@ -22,8 +22,6 @@ #include #include -#include -#include #include 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(idx_in_log_infoPerPTGs)) + ipp.lastDynState = m_lastSentVelCmd.ptg_dynState; + + ipp.dynState = ptg->getCurrentNavDynamicState(); } } // end "valid_TP" diff --git a/libs/nav/src/reactive/CLogFileRecord.cpp b/libs/nav/src/reactive/CLogFileRecord.cpp index 3f2c017b0c..ec2c09cddb 100644 --- a/libs/nav/src/reactive/CLogFileRecord.cpp +++ b/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(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(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 into CSerializable out << additional_debug_msgs; // v18 - navDynState.writeToStream(out); // v24 + // Removed v28. navDynState.writeToStream(out); // v24. out << visuals; // v27 } @@ -118,23 +115,7 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve { switch (version) { - case 0: - case 1: - case 2: - case 3: - case 4: - case 5: - case 6: - case 7: - case 8: - case 9: - case 10: - case 11: - case 12: - case 13: - case 14: - case 15: - case 16: + case 16: // removed support for ancient files (v<=15) case 17: case 18: case 19: @@ -146,17 +127,13 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve case 25: case 26: case 27: + case 28: { // Version 0 -------------- - uint32_t i, n; - infoPerPTG.clear(); - - in >> n; - infoPerPTG.resize(n); - for (i = 0; i < n; i++) + infoPerPTG.resize(in.ReadAs()); + for (auto& ipp : infoPerPTG) { - auto& ipp = infoPerPTG[i]; in >> ipp.PTG_desc; int32_t m; @@ -165,25 +142,17 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve if (m) in.ReadBufferFixEndianness(&(*ipp.TP_Obstacles.begin()), m); ipp.TP_Targets.clear(); - if (version >= 8) + if (version >= 26) { - if (version >= 26) - { - in >> ipp.TP_Targets; - } - else - { - mrpt::math::TPoint2D trg; - in >> trg; - ipp.TP_Targets.push_back(trg); - } + in >> ipp.TP_Targets; } else { - mrpt::poses::CPoint2D pos; - in >> pos; - ipp.TP_Targets.emplace_back(pos.x(), pos.y()); + mrpt::math::TPoint2D trg; + in >> trg; + ipp.TP_Targets.push_back(trg); } + if (version >= 17) in >> ipp.TP_Robot; else @@ -210,7 +179,7 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve in >> ipp.HLFR; - if (version >= 9) // Extra PTG info + // Extra PTG info (v9 { ipp.ptg.reset(); @@ -239,7 +208,14 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve { ipp.clearance.clear(); } - } + + // v28 + if (version >= 28) + { + ipp.dynState.readFromStream(in); + ipp.lastDynState.readFromStream(in); + } + } // end for each ipp in >> nSelectedPTG >> WS_Obstacles; if (version >= 20) @@ -264,210 +240,83 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve } WS_targets_relative.clear(); - if (version >= 8) + if (version >= 26) { - if (version >= 26) - { - in >> WS_targets_relative; - } - else - { - mrpt::math::TPoint2D trg; - in >> trg; - WS_targets_relative.emplace_back(trg); - } + in >> WS_targets_relative; } else { - mrpt::poses::CPoint2D pos; - in >> pos; - WS_targets_relative.emplace_back(mrpt::math::TPoint2D(pos.x(), pos.y())); + mrpt::math::TPoint2D trg; + in >> trg; + WS_targets_relative.emplace_back(trg); } - if (version >= 16) + in >> ptg_index_NOP >> ptg_last_k_NOP; + if (version >= 24) { - in >> ptg_index_NOP >> ptg_last_k_NOP; - if (version >= 24) - { - in >> rel_cur_pose_wrt_last_vel_cmd_NOP >> rel_pose_PTG_origin_wrt_sense_NOP; - } - else - { - mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP, - crel_pose_PTG_origin_wrt_sense_NOP; - in >> crel_cur_pose_wrt_last_vel_cmd_NOP >> crel_pose_PTG_origin_wrt_sense_NOP; - rel_cur_pose_wrt_last_vel_cmd_NOP = crel_cur_pose_wrt_last_vel_cmd_NOP.asTPose(); - rel_pose_PTG_origin_wrt_sense_NOP = crel_pose_PTG_origin_wrt_sense_NOP.asTPose(); - } + in >> rel_cur_pose_wrt_last_vel_cmd_NOP >> rel_pose_PTG_origin_wrt_sense_NOP; } else { - ptg_index_NOP = -1; + mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP, crel_pose_PTG_origin_wrt_sense_NOP; + in >> crel_cur_pose_wrt_last_vel_cmd_NOP >> crel_pose_PTG_origin_wrt_sense_NOP; + rel_cur_pose_wrt_last_vel_cmd_NOP = crel_cur_pose_wrt_last_vel_cmd_NOP.asTPose(); + rel_pose_PTG_origin_wrt_sense_NOP = crel_pose_PTG_origin_wrt_sense_NOP.asTPose(); } + 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) + else 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) - { - if (version >= 15) - { - if (ptg_index_NOP < 0) in >> cmd_vel; - } - else - { - std::vector vel; - in >> vel; - if (vel.size() == 2) - cmd_vel = mrpt::kinematics::CVehicleVelCmd::Ptr( - new mrpt::kinematics::CVehicleVelCmd_DiffDriven); - else - cmd_vel = - mrpt::kinematics::CVehicleVelCmd::Ptr(new mrpt::kinematics::CVehicleVelCmd_Holo); - for (size_t k = 0; k < cmd_vel->getVelCmdLength(); k++) - cmd_vel->setVelCmdElement(i, vel[k]); - } - } - else - { - float v, w; - in >> v >> w; - cmd_vel = - mrpt::kinematics::CVehicleVelCmd::Ptr(new mrpt::kinematics::CVehicleVelCmd_DiffDriven); - cmd_vel->setVelCmdElement(0, v); - cmd_vel->setVelCmdElement(0, w); - } + if (ptg_index_NOP < 0) in >> cmd_vel; if (version >= 15 && ptg_index_NOP < 0) in >> cmd_vel_original; - if (version < 13) { - float old_exec_time; - in >> old_exec_time; - values["executionTime"] = old_exec_time; - } - - if (version < 6) - { - mrpt::math::CVectorFloat prevV, prevW, prevSelPTG; - - // Previous values: (Removed in version 6) - in >> n; - prevV.resize(n); - if (n) in.ReadBufferFixEndianness(&(*prevV.begin()), n); - + uint32_t n; in >> n; - prevW.resize(n); - if (n) in.ReadBufferFixEndianness(&(*prevW.begin()), n); - - in >> n; - prevSelPTG.resize(n); - if (n) in.ReadBufferFixEndianness(&(*prevSelPTG.begin()), n); - } - - in >> n; - robotShape_x.resize(n); - robotShape_y.resize(n); - if (n) - { - in.ReadBufferFixEndianness(&(*robotShape_x.begin()), n); - in.ReadBufferFixEndianness(&(*robotShape_y.begin()), n); - } - - if (version > 0) - { // Version 1 -------------- - if (version >= 10) + robotShape_x.resize(n); + robotShape_y.resize(n); + if (n) { - in >> cur_vel >> cur_vel_local; - } - else - { - float actual_v, actual_w; - in >> actual_v >> actual_w; - cur_vel = mrpt::math::TTwist2D(0, 0, 0); - cur_vel_local = mrpt::math::TTwist2D(actual_v, .0, actual_w); + in.ReadBufferFixEndianness(&(*robotShape_x.begin()), n); + in.ReadBufferFixEndianness(&(*robotShape_y.begin()), n); } } - else - { // Default values for old versions: - cur_vel = mrpt::math::TTwist2D(0, 0, 0); - } - if (version < 13 && version > 1) - { - float old_estim_period; - in >> old_estim_period; - values["estimatedExecutionPeriod"] = old_estim_period; - } + in >> cur_vel >> cur_vel_local; - for (i = 0; i < infoPerPTG.size(); i++) - { - infoPerPTG[i].evalFactors.clear(); - } - if (version > 2) + for (auto& ipp : infoPerPTG) ipp.evalFactors.clear(); + + for (auto& ipp : infoPerPTG) { - // Version 3..22 ---------- - for (i = 0; i < infoPerPTG.size(); i++) + if (version < 22) { - if (version < 22) - { - in >> n; - for (unsigned int j = 0; j < n; j++) - { - float f; - in >> f; - infoPerPTG[i].evalFactors[mrpt::format("f%u", j)] = f; - } - } - else + const auto n = in.ReadAs(); + for (unsigned int j = 0; j < n; j++) { - in >> infoPerPTG[i].evalFactors; + float f; + in >> f; + ipp.evalFactors[mrpt::format("f%u", j)] = f; } } - } - - if (version > 3) - { - // Version 4 ---------- - in >> nPTGs; - if (version < 9) // Old "securityDistances", now unused. - { - in >> n; - float dummy; - for (i = 0; i < n; i++) in >> dummy; - } - } - else - { - nPTGs = infoPerPTG.size(); - } - - if (version > 4) - { - if (version < 10) - { - int32_t navigatorBehavior; // removed in v10 - in >> navigatorBehavior; - } - - if (version < 6) // Removed in version 6: + else { - mrpt::poses::CPoint2D doorCrossing_P1, doorCrossing_P2; - in >> doorCrossing_P1 >> doorCrossing_P2; + in >> ipp.evalFactors; } } - if (version > 6 && version < 13) - { - mrpt::system::TTimeStamp tt; - in >> tt; - timestamps["tim_start_iteration"] = tt; - } + // Version 4 ---------- + in >> nPTGs; if (version >= 11) { @@ -478,12 +327,6 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve robotShape_radius = 0.5; } - if (version >= 12 && version < 15) - { - std::vector> dummy_cmd_vel_filterings; - in >> dummy_cmd_vel_filterings; - } - if (version >= 13) { in >> values >> timestamps; @@ -494,23 +337,16 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve timestamps.clear(); } - if (version >= 14) + if (version >= 24) { - if (version >= 24) - { - in >> relPoseSense >> relPoseVelCmd; - } - else - { - mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd; - in >> crelPoseSense >> crelPoseVelCmd; - relPoseSense = crelPoseSense.asTPose(); - relPoseVelCmd = crelPoseVelCmd.asTPose(); - } + in >> relPoseSense >> relPoseVelCmd; } else { - relPoseSense = relPoseVelCmd = mrpt::math::TPose2D(0, 0, 0); + mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd; + in >> crelPoseSense >> crelPoseVelCmd; + relPoseSense = crelPoseSense.asTPose(); + relPoseVelCmd = crelPoseVelCmd.asTPose(); } if (version >= 18) @@ -518,13 +354,17 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve else additional_debug_msgs.clear(); - if (version >= 24) - navDynState.readFromStream(in); - else + if (version >= 24 && version < 28) + { + mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds; + for (auto& ipp : infoPerPTG) ipp.dynState = ds; + } + else if (version < 24) { - navDynState = CParameterizedTrajectoryGenerator::TNavDynamicState(); - navDynState.curVelLocal = cur_vel_local; - if (!WS_targets_relative.empty()) navDynState.relTarget = WS_targets_relative[0]; + mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds; + 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) diff --git a/libs/nav/src/tpspace/CParameterizedTrajectoryGenerator.cpp b/libs/nav/src/tpspace/CParameterizedTrajectoryGenerator.cpp index 881d0455ef..f443ba2562 100644 --- a/libs/nav/src/tpspace/CParameterizedTrajectoryGenerator.cpp +++ b/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); From e3366f919c2d4d055aa6021b7df9230509bb4a0a Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 4 Oct 2024 15:59:40 +0200 Subject: [PATCH 2/4] update pymrpt --- python/src/mrpt/maps/CVoxelMapBase.cpp | 4 ++-- .../mrpt/nav/holonomic/ClearanceDiagram.cpp | 7 +++--- .../src/mrpt/nav/reactive/CLogFileRecord.cpp | 4 ++-- .../CParameterizedTrajectoryGenerator.cpp | 4 ++-- python/src/mrpt/opengl/Texture.cpp | 15 +++++++++--- python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi | 6 +++-- .../mrpt/pymrpt/mrpt/opengl/__init__.pyi | 23 +++++++++++++++++++ 7 files changed, 49 insertions(+), 14 deletions(-) diff --git a/python/src/mrpt/maps/CVoxelMapBase.cpp b/python/src/mrpt/maps/CVoxelMapBase.cpp index 8d31660086..54e2b6908a 100644 --- a/python/src/mrpt/maps/CVoxelMapBase.cpp +++ b/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_, std::shared_ptr>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t", ""); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::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_, std::shared_ptr>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t", ""); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); diff --git a/python/src/mrpt/nav/holonomic/ClearanceDiagram.cpp b/python/src/mrpt/nav/holonomic/ClearanceDiagram.cpp index 48f38c2dce..05dba35259 100644 --- a/python/src/mrpt/nav/holonomic/ClearanceDiagram.cpp +++ b/python/src/mrpt/nav/holonomic/ClearanceDiagram.cpp @@ -71,7 +71,7 @@ void bind_mrpt_nav_holonomic_ClearanceDiagram(std::function< pybind11::module &( cl.def("decimated_k_to_real_k", (size_t (mrpt::nav::ClearanceDiagram::*)(size_t) const) &mrpt::nav::ClearanceDiagram::decimated_k_to_real_k, "C++: mrpt::nav::ClearanceDiagram::decimated_k_to_real_k(size_t) const --> size_t", pybind11::arg("k")); cl.def("assign", (class mrpt::nav::ClearanceDiagram & (mrpt::nav::ClearanceDiagram::*)(const class mrpt::nav::ClearanceDiagram &)) &mrpt::nav::ClearanceDiagram::operator=, "C++: mrpt::nav::ClearanceDiagram::operator=(const class mrpt::nav::ClearanceDiagram &) --> class mrpt::nav::ClearanceDiagram &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - // mrpt::nav::PTG_collision_behavior_t file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:42 + // mrpt::nav::PTG_collision_behavior_t file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:43 pybind11::enum_(M("mrpt::nav"), "PTG_collision_behavior_t", pybind11::arithmetic(), "Defines behaviors for where there is an obstacle *inside* the robot shape\nright at the beginning of a PTG trajectory.\n\n\n \n Used in CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR") .value("COLL_BEH_BACK_AWAY", mrpt::nav::COLL_BEH_BACK_AWAY) .value("COLL_BEH_STOP", mrpt::nav::COLL_BEH_STOP) @@ -79,7 +79,7 @@ void bind_mrpt_nav_holonomic_ClearanceDiagram(std::function< pybind11::module &( ; - { // mrpt::nav::CParameterizedTrajectoryGenerator file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:79 + { // mrpt::nav::CParameterizedTrajectoryGenerator file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:80 pybind11::class_, mrpt::serialization::CSerializable, mrpt::config::CLoadableOptions> cl(M("mrpt::nav"), "CParameterizedTrajectoryGenerator", "This is the base class for any user-defined PTG.\n There is a class factory interface in\nCParameterizedTrajectoryGenerator::CreatePTG.\n\n Papers:\n - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, \"Extending\nObstacle Avoidance Methods through Multiple Parameter-Space Transformations\",\nAutonomous Robots, vol. 24, no. 1, 2008.\nhttp://ingmec.ual.es/~jlblanco/papers/blanco2008eoa_DRAFT.pdf\n\n Changes history:\n - 30/JUN/2004: Creation (JLBC)\n - 16/SEP/2004: Totally redesigned.\n - 15/SEP/2005: Totally rewritten again, for integration into MRPT\nApplications Repository.\n - 19/JUL/2009: Simplified to use only STL data types, and created the class\nfactory interface.\n - MAY/2016: Refactored into CParameterizedTrajectoryGenerator,\nCPTG_DiffDrive_CollisionGridBased, PTG classes renamed.\n - 2016-2018: Many features added to support \"PTG continuation\", dynamic\npaths depending on vehicle speeds, etc.\n\n \n\n "); cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::nav::CParameterizedTrajectoryGenerator::*)() const) &mrpt::nav::CParameterizedTrajectoryGenerator::GetRuntimeClass, "C++: mrpt::nav::CParameterizedTrajectoryGenerator::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::nav::CParameterizedTrajectoryGenerator::GetRuntimeClassIdStatic, "C++: mrpt::nav::CParameterizedTrajectoryGenerator::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); @@ -148,7 +148,7 @@ void bind_mrpt_nav_holonomic_ClearanceDiagram(std::function< pybind11::module &( cl.def("evalClearanceSingleObstacle", (void (mrpt::nav::CParameterizedTrajectoryGenerator::*)(const double, const double, const unsigned short, class std::map &, bool) const) &mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle, "Evals the robot clearance for each robot pose along path `k`, for the\n real distances in\n the key of the map<>, then keep in the map value the minimum of its\n current stored clearance,\n or the computed clearance. In case of collision, clearance is zero.\n \n\n true: normal use for obstacles; false: compute\n shortest distances to a target point (no collision)\n\nC++: mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle(const double, const double, const unsigned short, class std::map &, bool) const --> void", pybind11::arg("ox"), pybind11::arg("oy"), pybind11::arg("k"), pybind11::arg("inout_realdist2clearance"), pybind11::arg("treat_as_obstacle")); cl.def("assign", (class mrpt::nav::CParameterizedTrajectoryGenerator & (mrpt::nav::CParameterizedTrajectoryGenerator::*)(const class mrpt::nav::CParameterizedTrajectoryGenerator &)) &mrpt::nav::CParameterizedTrajectoryGenerator::operator=, "C++: mrpt::nav::CParameterizedTrajectoryGenerator::operator=(const class mrpt::nav::CParameterizedTrajectoryGenerator &) --> class mrpt::nav::CParameterizedTrajectoryGenerator &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:176 + { // mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:177 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TNavDynamicState", "Dynamic state that may affect the PTG path parameterization. \n\n nav_reactive "); cl.def( pybind11::init( [](){ return new mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState(); } ) ); @@ -156,6 +156,7 @@ void bind_mrpt_nav_holonomic_ClearanceDiagram(std::function< pybind11::module &( cl.def_readwrite("curVelLocal", &mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::curVelLocal); cl.def_readwrite("relTarget", &mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::relTarget); cl.def_readwrite("targetRelSpeed", &mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::targetRelSpeed); + cl.def_readwrite("internalState", &mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::internalState); cl.def("__eq__", (bool (mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::*)(const struct mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState &) const) &mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::operator==, "C++: mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::operator==(const struct mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState &) const --> bool", pybind11::arg("o")); cl.def("__ne__", (bool (mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::*)(const struct mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState &) const) &mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::operator!=, "C++: mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::operator!=(const struct mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState &) const --> bool", pybind11::arg("o")); cl.def("writeToStream", (void (mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::*)(class mrpt::serialization::CArchive &) const) &mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::writeToStream, "C++: mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::writeToStream(class mrpt::serialization::CArchive &) const --> void", pybind11::arg("out")); diff --git a/python/src/mrpt/nav/reactive/CLogFileRecord.cpp b/python/src/mrpt/nav/reactive/CLogFileRecord.cpp index 6b77bedce4..b23c05004b 100644 --- a/python/src/mrpt/nav/reactive/CLogFileRecord.cpp +++ b/python/src/mrpt/nav/reactive/CLogFileRecord.cpp @@ -141,7 +141,6 @@ void bind_mrpt_nav_reactive_CLogFileRecord(std::function< pybind11::module &(std cl.def( pybind11::init( [](){ return new mrpt::nav::CLogFileRecord(); }, [](){ return new PyCallBack_mrpt_nav_CLogFileRecord(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_nav_CLogFileRecord const &o){ return new PyCallBack_mrpt_nav_CLogFileRecord(o); } ) ); cl.def( pybind11::init( [](mrpt::nav::CLogFileRecord const &o){ return new mrpt::nav::CLogFileRecord(o); } ) ); - cl.def_readwrite("navDynState", &mrpt::nav::CLogFileRecord::navDynState); cl.def_readwrite("nPTGs", &mrpt::nav::CLogFileRecord::nPTGs); cl.def_readwrite("infoPerPTG", &mrpt::nav::CLogFileRecord::infoPerPTG); cl.def_readwrite("nSelectedPTG", &mrpt::nav::CLogFileRecord::nSelectedPTG); @@ -166,7 +165,6 @@ void bind_mrpt_nav_reactive_CLogFileRecord(std::function< pybind11::module &(std cl.def_readwrite("ptg_last_k_NOP", &mrpt::nav::CLogFileRecord::ptg_last_k_NOP); cl.def_readwrite("rel_cur_pose_wrt_last_vel_cmd_NOP", &mrpt::nav::CLogFileRecord::rel_cur_pose_wrt_last_vel_cmd_NOP); cl.def_readwrite("rel_pose_PTG_origin_wrt_sense_NOP", &mrpt::nav::CLogFileRecord::rel_pose_PTG_origin_wrt_sense_NOP); - cl.def_readwrite("ptg_last_navDynState", &mrpt::nav::CLogFileRecord::ptg_last_navDynState); cl.def_readwrite("visuals", &mrpt::nav::CLogFileRecord::visuals); cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::nav::CLogFileRecord::GetRuntimeClassIdStatic, "C++: mrpt::nav::CLogFileRecord::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::nav::CLogFileRecord::*)() const) &mrpt::nav::CLogFileRecord::GetRuntimeClass, "C++: mrpt::nav::CLogFileRecord::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); @@ -192,6 +190,8 @@ void bind_mrpt_nav_reactive_CLogFileRecord(std::function< pybind11::module &(std cl.def_readwrite("HLFR", &mrpt::nav::CLogFileRecord::TInfoPerPTG::HLFR); cl.def_readwrite("ptg", &mrpt::nav::CLogFileRecord::TInfoPerPTG::ptg); cl.def_readwrite("clearance", &mrpt::nav::CLogFileRecord::TInfoPerPTG::clearance); + cl.def_readwrite("dynState", &mrpt::nav::CLogFileRecord::TInfoPerPTG::dynState); + cl.def_readwrite("lastDynState", &mrpt::nav::CLogFileRecord::TInfoPerPTG::lastDynState); cl.def("assign", (struct mrpt::nav::CLogFileRecord::TInfoPerPTG & (mrpt::nav::CLogFileRecord::TInfoPerPTG::*)(const struct mrpt::nav::CLogFileRecord::TInfoPerPTG &)) &mrpt::nav::CLogFileRecord::TInfoPerPTG::operator=, "C++: mrpt::nav::CLogFileRecord::TInfoPerPTG::operator=(const struct mrpt::nav::CLogFileRecord::TInfoPerPTG &) --> struct mrpt::nav::CLogFileRecord::TInfoPerPTG &", pybind11::return_value_policy::automatic, pybind11::arg("")); } diff --git a/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp b/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp index 1bf7b77730..ea3c56c281 100644 --- a/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp +++ b/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp @@ -273,7 +273,7 @@ struct PyCallBack_mrpt_nav_CAbstractHolonomicReactiveMethod : public mrpt::nav:: void bind_mrpt_nav_tpspace_CParameterizedTrajectoryGenerator(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::nav::CPTG_RobotShape_Polygonal file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:501 + { // mrpt::nav::CPTG_RobotShape_Polygonal file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:505 pybind11::class_, mrpt::nav::CParameterizedTrajectoryGenerator> cl(M("mrpt::nav"), "CPTG_RobotShape_Polygonal", "Base class for all PTGs using a 2D polygonal robot shape model.\n \n\n\n "); cl.def("setRobotShape", (void (mrpt::nav::CPTG_RobotShape_Polygonal::*)(const class mrpt::math::CPolygon &)) &mrpt::nav::CPTG_RobotShape_Polygonal::setRobotShape, "@{ *\n\n Robot shape must be set before initialization, either from ctor params\n or via this method. \n\nC++: mrpt::nav::CPTG_RobotShape_Polygonal::setRobotShape(const class mrpt::math::CPolygon &) --> void", pybind11::arg("robotShape")); cl.def("getRobotShape", (const class mrpt::math::CPolygon & (mrpt::nav::CPTG_RobotShape_Polygonal::*)() const) &mrpt::nav::CPTG_RobotShape_Polygonal::getRobotShape, "C++: mrpt::nav::CPTG_RobotShape_Polygonal::getRobotShape() const --> const class mrpt::math::CPolygon &", pybind11::return_value_policy::automatic); @@ -285,7 +285,7 @@ void bind_mrpt_nav_tpspace_CParameterizedTrajectoryGenerator(std::function< pybi cl.def_static("static_add_robotShape_to_setOfLines", (void (*)(class mrpt::opengl::CSetOfLines &, const class mrpt::poses::CPose2D &, const class mrpt::math::CPolygon &)) &mrpt::nav::CPTG_RobotShape_Polygonal::static_add_robotShape_to_setOfLines, "C++: mrpt::nav::CPTG_RobotShape_Polygonal::static_add_robotShape_to_setOfLines(class mrpt::opengl::CSetOfLines &, const class mrpt::poses::CPose2D &, const class mrpt::math::CPolygon &) --> void", pybind11::arg("gl_shape"), pybind11::arg("origin"), pybind11::arg("robotShape")); cl.def("assign", (class mrpt::nav::CPTG_RobotShape_Polygonal & (mrpt::nav::CPTG_RobotShape_Polygonal::*)(const class mrpt::nav::CPTG_RobotShape_Polygonal &)) &mrpt::nav::CPTG_RobotShape_Polygonal::operator=, "C++: mrpt::nav::CPTG_RobotShape_Polygonal::operator=(const class mrpt::nav::CPTG_RobotShape_Polygonal &) --> class mrpt::nav::CPTG_RobotShape_Polygonal &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::nav::CPTG_RobotShape_Circular file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:545 + { // mrpt::nav::CPTG_RobotShape_Circular file:mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h line:549 pybind11::class_, mrpt::nav::CParameterizedTrajectoryGenerator> cl(M("mrpt::nav"), "CPTG_RobotShape_Circular", "Base class for all PTGs using a 2D circular robot shape model.\n \n\n\n "); cl.def("setRobotShapeRadius", (void (mrpt::nav::CPTG_RobotShape_Circular::*)(const double)) &mrpt::nav::CPTG_RobotShape_Circular::setRobotShapeRadius, "@{ *\n\n Robot shape must be set before initialization, either from ctor params\n or via this method. \n\nC++: mrpt::nav::CPTG_RobotShape_Circular::setRobotShapeRadius(const double) --> void", pybind11::arg("robot_radius")); cl.def("getRobotShapeRadius", (double (mrpt::nav::CPTG_RobotShape_Circular::*)() const) &mrpt::nav::CPTG_RobotShape_Circular::getRobotShapeRadius, "C++: mrpt::nav::CPTG_RobotShape_Circular::getRobotShapeRadius() const --> double"); diff --git a/python/src/mrpt/opengl/Texture.cpp b/python/src/mrpt/opengl/Texture.cpp index 16a2171f8e..d9281aa075 100644 --- a/python/src/mrpt/opengl/Texture.cpp +++ b/python/src/mrpt/opengl/Texture.cpp @@ -52,6 +52,13 @@ void bind_mrpt_opengl_Texture(std::function< pybind11::module &(std::string cons pybind11::class_> cl(M("mrpt::opengl"), "Texture", "Resource management for OpenGL 2D or Cube textures.\n\n The texture is generated when images are assigned via\n assignImage2D() or assignCubeImages().\n\n \n CRenderizableShaderTexturedTriangles\n \n\n\n "); cl.def( pybind11::init( [](){ return new mrpt::opengl::Texture(); } ) ); cl.def( pybind11::init( [](mrpt::opengl::Texture const &o){ return new mrpt::opengl::Texture(o); } ) ); + + pybind11::enum_(cl, "Wrapping", "") + .value("Repeat", mrpt::opengl::Texture::Wrapping::Repeat) + .value("MirroredRepeat", mrpt::opengl::Texture::Wrapping::MirroredRepeat) + .value("ClampToEdge", mrpt::opengl::Texture::Wrapping::ClampToEdge) + .value("ClapToBorder", mrpt::opengl::Texture::Wrapping::ClapToBorder); + cl.def("assignImage2D", [](mrpt::opengl::Texture &o, const class mrpt::img::CImage & a0, const struct mrpt::opengl::Texture::Options & a1) -> void { return o.assignImage2D(a0, a1); }, "", pybind11::arg("rgb"), pybind11::arg("o")); cl.def("assignImage2D", (void (mrpt::opengl::Texture::*)(const class mrpt::img::CImage &, const struct mrpt::opengl::Texture::Options &, int)) &mrpt::opengl::Texture::assignImage2D, "This is how an 2D texture image is loaded into this object, and a\n texture ID is generated underneath. Valid image formats are 8bit per\n channel RGB or RGBA.\n\nC++: mrpt::opengl::Texture::assignImage2D(const class mrpt::img::CImage &, const struct mrpt::opengl::Texture::Options &, int) --> void", pybind11::arg("rgb"), pybind11::arg("o"), pybind11::arg("textureUnit")); cl.def("assignImage2D", [](mrpt::opengl::Texture &o, const class mrpt::img::CImage & a0, const class mrpt::img::CImage & a1, const struct mrpt::opengl::Texture::Options & a2) -> void { return o.assignImage2D(a0, a1, a2); }, "", pybind11::arg("rgb"), pybind11::arg("alpha"), pybind11::arg("o")); @@ -66,20 +73,22 @@ void bind_mrpt_opengl_Texture(std::function< pybind11::module &(std::string cons cl.def("textureNameID", (unsigned int (mrpt::opengl::Texture::*)() const) &mrpt::opengl::Texture::textureNameID, "C++: mrpt::opengl::Texture::textureNameID() const --> unsigned int"); cl.def("assign", (class mrpt::opengl::Texture & (mrpt::opengl::Texture::*)(const class mrpt::opengl::Texture &)) &mrpt::opengl::Texture::operator=, "C++: mrpt::opengl::Texture::operator=(const class mrpt::opengl::Texture &) --> class mrpt::opengl::Texture &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::opengl::Texture::Options file:mrpt/opengl/Texture.h line:47 + { // mrpt::opengl::Texture::Options file:mrpt/opengl/Texture.h line:55 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "Options", "Options while creating a texture from an image."); cl.def( pybind11::init( [](){ return new mrpt::opengl::Texture::Options(); } ) ); cl.def_readwrite("generateMipMaps", &mrpt::opengl::Texture::Options::generateMipMaps); cl.def_readwrite("magnifyLinearFilter", &mrpt::opengl::Texture::Options::magnifyLinearFilter); cl.def_readwrite("enableTransparency", &mrpt::opengl::Texture::Options::enableTransparency); + cl.def_readwrite("wrappingModeS", &mrpt::opengl::Texture::Options::wrappingModeS); + cl.def_readwrite("wrappingModeT", &mrpt::opengl::Texture::Options::wrappingModeT); } } - // mrpt::opengl::getNewTextureNumber() file:mrpt/opengl/Texture.h line:118 + // mrpt::opengl::getNewTextureNumber() file:mrpt/opengl/Texture.h line:132 M("mrpt::opengl").def("getNewTextureNumber", (unsigned int (*)()) &mrpt::opengl::getNewTextureNumber, "C++: mrpt::opengl::getNewTextureNumber() --> unsigned int"); - // mrpt::opengl::releaseTextureName(const unsigned int &) file:mrpt/opengl/Texture.h line:119 + // mrpt::opengl::releaseTextureName(const unsigned int &) file:mrpt/opengl/Texture.h line:133 M("mrpt::opengl").def("releaseTextureName", (void (*)(const unsigned int &)) &mrpt::opengl::releaseTextureName, "C++: mrpt::opengl::releaseTextureName(const unsigned int &) --> void", pybind11::arg("t")); { // mrpt::opengl::CRenderizableShaderTexturedTriangles file:mrpt/opengl/CRenderizableShaderTexturedTriangles.h line:28 diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi index d0fa18b568..a8db2db8ff 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi @@ -2,6 +2,7 @@ from typing import Any, ClassVar, Dict, List, Optional from typing import overload import mrpt.pymrpt.mrpt.config +import mrpt.pymrpt.mrpt.containers import mrpt.pymrpt.mrpt.graphs import mrpt.pymrpt.mrpt.img import mrpt.pymrpt.mrpt.kinematics @@ -598,8 +599,10 @@ class CLogFileRecord(mrpt.pymrpt.mrpt.serialization.CSerializable): clearance: ClearanceDiagram desiredDirection: float desiredSpeed: float + dynState: CParameterizedTrajectoryGenerator.TNavDynamicState evalFactors: Dict[str,float] evaluation: float + lastDynState: CParameterizedTrajectoryGenerator.TNavDynamicState ptg: CParameterizedTrajectoryGenerator timeForHolonomicMethod: float timeForTPObsTransformation: float @@ -619,10 +622,8 @@ class CLogFileRecord(mrpt.pymrpt.mrpt.serialization.CSerializable): infoPerPTG: Any nPTGs: int nSelectedPTG: int - navDynState: CParameterizedTrajectoryGenerator.TNavDynamicState ptg_index_NOP: int ptg_last_k_NOP: int - ptg_last_navDynState: CParameterizedTrajectoryGenerator.TNavDynamicState relPoseSense: mrpt.pymrpt.mrpt.math.TPose2D relPoseVelCmd: mrpt.pymrpt.mrpt.math.TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP: mrpt.pymrpt.mrpt.math.TPose2D @@ -1106,6 +1107,7 @@ class CParameterizedTrajectoryGenerator(mrpt.pymrpt.mrpt.serialization.CSerializ class TNavDynamicState: __hash__: ClassVar[None] = ... curVelLocal: Any + internalState: mrpt.pymrpt.mrpt.containers.yaml relTarget: mrpt.pymrpt.mrpt.math.TPose2D targetRelSpeed: float @overload diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi index 136283fa61..fd21ef1880 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi @@ -3802,7 +3802,30 @@ class Texture: enableTransparency: bool generateMipMaps: bool magnifyLinearFilter: bool + wrappingModeS: Texture.Wrapping + wrappingModeT: Texture.Wrapping def __init__(self) -> None: ... + + class Wrapping: + __doc__: ClassVar[str] = ... # read-only + __members__: ClassVar[dict] = ... # read-only + ClampToEdge: ClassVar[Texture.Wrapping] = ... + ClapToBorder: ClassVar[Texture.Wrapping] = ... + MirroredRepeat: ClassVar[Texture.Wrapping] = ... + Repeat: ClassVar[Texture.Wrapping] = ... + __entries: ClassVar[dict] = ... + def __init__(self, value: int) -> None: ... + def __eq__(self, other: object) -> bool: ... + def __getstate__(self) -> int: ... + def __hash__(self) -> int: ... + def __index__(self) -> int: ... + def __int__(self) -> int: ... + def __ne__(self, other: object) -> bool: ... + def __setstate__(self, state: int) -> None: ... + @property + def name(self) -> str: ... + @property + def value(self) -> int: ... @overload def __init__(self) -> None: ... @overload From 805bccb42940d7588ae64ef15d76a9011ce304aa Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 4 Oct 2024 19:27:27 +0200 Subject: [PATCH 3/4] Fix deserialization --- libs/nav/src/reactive/CLogFileRecord.cpp | 298 ++++++++++++++++++----- 1 file changed, 236 insertions(+), 62 deletions(-) diff --git a/libs/nav/src/reactive/CLogFileRecord.cpp b/libs/nav/src/reactive/CLogFileRecord.cpp index ec2c09cddb..99603936ca 100644 --- a/libs/nav/src/reactive/CLogFileRecord.cpp +++ b/libs/nav/src/reactive/CLogFileRecord.cpp @@ -115,7 +115,23 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve { switch (version) { - case 16: // removed support for ancient files (v<=15) + case 0: + case 1: + case 2: + case 3: + case 4: + case 5: + case 6: + case 7: + case 8: + case 9: + case 10: + case 11: + case 12: + case 13: + case 14: + case 15: + case 16: case 17: case 18: case 19: @@ -130,10 +146,15 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve case 28: { // Version 0 -------------- + uint32_t i, n; + infoPerPTG.clear(); - infoPerPTG.resize(in.ReadAs()); - for (auto& ipp : infoPerPTG) + + in >> n; + infoPerPTG.resize(n); + for (i = 0; i < n; i++) { + auto& ipp = infoPerPTG[i]; in >> ipp.PTG_desc; int32_t m; @@ -142,17 +163,25 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve if (m) in.ReadBufferFixEndianness(&(*ipp.TP_Obstacles.begin()), m); ipp.TP_Targets.clear(); - if (version >= 26) + if (version >= 8) { - in >> ipp.TP_Targets; + if (version >= 26) + { + in >> ipp.TP_Targets; + } + else + { + mrpt::math::TPoint2D trg; + in >> trg; + ipp.TP_Targets.push_back(trg); + } } else { - mrpt::math::TPoint2D trg; - in >> trg; - ipp.TP_Targets.push_back(trg); + mrpt::poses::CPoint2D pos; + in >> pos; + ipp.TP_Targets.emplace_back(pos.x(), pos.y()); } - if (version >= 17) in >> ipp.TP_Robot; else @@ -179,7 +208,7 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve in >> ipp.HLFR; - // Extra PTG info (v9 + if (version >= 9) // Extra PTG info { ipp.ptg.reset(); @@ -209,13 +238,12 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve ipp.clearance.clear(); } - // v28 if (version >= 28) { ipp.dynState.readFromStream(in); ipp.lastDynState.readFromStream(in); } - } // end for each ipp + } in >> nSelectedPTG >> WS_Obstacles; if (version >= 20) @@ -240,83 +268,214 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve } WS_targets_relative.clear(); - if (version >= 26) + if (version >= 8) { - in >> WS_targets_relative; + if (version >= 26) + { + in >> WS_targets_relative; + } + else + { + mrpt::math::TPoint2D trg; + in >> trg; + WS_targets_relative.emplace_back(trg); + } } else { - mrpt::math::TPoint2D trg; - in >> trg; - WS_targets_relative.emplace_back(trg); + mrpt::poses::CPoint2D pos; + in >> pos; + WS_targets_relative.emplace_back(mrpt::math::TPoint2D(pos.x(), pos.y())); } - in >> ptg_index_NOP >> ptg_last_k_NOP; - if (version >= 24) + if (version >= 16) { - in >> rel_cur_pose_wrt_last_vel_cmd_NOP >> rel_pose_PTG_origin_wrt_sense_NOP; + in >> ptg_index_NOP >> ptg_last_k_NOP; + if (version >= 24) + { + in >> rel_cur_pose_wrt_last_vel_cmd_NOP >> rel_pose_PTG_origin_wrt_sense_NOP; + } + else + { + mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP, + crel_pose_PTG_origin_wrt_sense_NOP; + in >> crel_cur_pose_wrt_last_vel_cmd_NOP >> crel_pose_PTG_origin_wrt_sense_NOP; + rel_cur_pose_wrt_last_vel_cmd_NOP = crel_cur_pose_wrt_last_vel_cmd_NOP.asTPose(); + rel_pose_PTG_origin_wrt_sense_NOP = crel_pose_PTG_origin_wrt_sense_NOP.asTPose(); + } } else { - mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP, crel_pose_PTG_origin_wrt_sense_NOP; - in >> crel_cur_pose_wrt_last_vel_cmd_NOP >> crel_pose_PTG_origin_wrt_sense_NOP; - rel_cur_pose_wrt_last_vel_cmd_NOP = crel_cur_pose_wrt_last_vel_cmd_NOP.asTPose(); - rel_pose_PTG_origin_wrt_sense_NOP = crel_pose_PTG_origin_wrt_sense_NOP.asTPose(); + ptg_index_NOP = -1; } - if (version >= 17 && version < 24) { mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds; in >> ds.curVelLocal; for (auto& ipp : infoPerPTG) ipp.dynState = ds; } - else if (version >= 24 && version < 28) + if (version >= 24 && version < 28) { mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds; ds.readFromStream(in); for (auto& ipp : infoPerPTG) ipp.dynState = ds; } - if (ptg_index_NOP < 0) in >> cmd_vel; + if (version >= 10) + { + if (version >= 15) + { + if (ptg_index_NOP < 0) in >> cmd_vel; + } + else + { + std::vector vel; + in >> vel; + if (vel.size() == 2) + cmd_vel = mrpt::kinematics::CVehicleVelCmd::Ptr( + new mrpt::kinematics::CVehicleVelCmd_DiffDriven); + else + cmd_vel = + mrpt::kinematics::CVehicleVelCmd::Ptr(new mrpt::kinematics::CVehicleVelCmd_Holo); + for (size_t k = 0; k < cmd_vel->getVelCmdLength(); k++) + cmd_vel->setVelCmdElement(i, vel[k]); + } + } + else + { + float v, w; + in >> v >> w; + cmd_vel = + mrpt::kinematics::CVehicleVelCmd::Ptr(new mrpt::kinematics::CVehicleVelCmd_DiffDriven); + cmd_vel->setVelCmdElement(0, v); + cmd_vel->setVelCmdElement(0, w); + } if (version >= 15 && ptg_index_NOP < 0) in >> cmd_vel_original; + if (version < 13) { - uint32_t n; + float old_exec_time; + in >> old_exec_time; + values["executionTime"] = old_exec_time; + } + + if (version < 6) + { + mrpt::math::CVectorFloat prevV, prevW, prevSelPTG; + + // Previous values: (Removed in version 6) + in >> n; + prevV.resize(n); + if (n) in.ReadBufferFixEndianness(&(*prevV.begin()), n); + in >> n; - robotShape_x.resize(n); - robotShape_y.resize(n); - if (n) + prevW.resize(n); + if (n) in.ReadBufferFixEndianness(&(*prevW.begin()), n); + + in >> n; + prevSelPTG.resize(n); + if (n) in.ReadBufferFixEndianness(&(*prevSelPTG.begin()), n); + } + + in >> n; + robotShape_x.resize(n); + robotShape_y.resize(n); + if (n) + { + in.ReadBufferFixEndianness(&(*robotShape_x.begin()), n); + in.ReadBufferFixEndianness(&(*robotShape_y.begin()), n); + } + + if (version > 0) + { // Version 1 -------------- + if (version >= 10) + { + in >> cur_vel >> cur_vel_local; + } + else { - in.ReadBufferFixEndianness(&(*robotShape_x.begin()), n); - in.ReadBufferFixEndianness(&(*robotShape_y.begin()), n); + float actual_v, actual_w; + in >> actual_v >> actual_w; + cur_vel = mrpt::math::TTwist2D(0, 0, 0); + cur_vel_local = mrpt::math::TTwist2D(actual_v, .0, actual_w); } } + else + { // Default values for old versions: + cur_vel = mrpt::math::TTwist2D(0, 0, 0); + } - in >> cur_vel >> cur_vel_local; - - for (auto& ipp : infoPerPTG) ipp.evalFactors.clear(); + if (version < 13 && version > 1) + { + float old_estim_period; + in >> old_estim_period; + values["estimatedExecutionPeriod"] = old_estim_period; + } - for (auto& ipp : infoPerPTG) + for (i = 0; i < infoPerPTG.size(); i++) + { + infoPerPTG[i].evalFactors.clear(); + } + if (version > 2) { - if (version < 22) + // Version 3..22 ---------- + for (i = 0; i < infoPerPTG.size(); i++) { - const auto n = in.ReadAs(); - for (unsigned int j = 0; j < n; j++) + if (version < 22) { - float f; - in >> f; - ipp.evalFactors[mrpt::format("f%u", j)] = f; + in >> n; + for (unsigned int j = 0; j < n; j++) + { + float f; + in >> f; + infoPerPTG[i].evalFactors[mrpt::format("f%u", j)] = f; + } + } + else + { + in >> infoPerPTG[i].evalFactors; } } - else + } + + if (version > 3) + { + // Version 4 ---------- + in >> nPTGs; + if (version < 9) // Old "securityDistances", now unused. + { + in >> n; + float dummy; + for (i = 0; i < n; i++) in >> dummy; + } + } + else + { + nPTGs = infoPerPTG.size(); + } + + if (version > 4) + { + if (version < 10) + { + int32_t navigatorBehavior; // removed in v10 + in >> navigatorBehavior; + } + + if (version < 6) // Removed in version 6: { - in >> ipp.evalFactors; + mrpt::poses::CPoint2D doorCrossing_P1, doorCrossing_P2; + in >> doorCrossing_P1 >> doorCrossing_P2; } } - // Version 4 ---------- - in >> nPTGs; + if (version > 6 && version < 13) + { + mrpt::system::TTimeStamp tt; + in >> tt; + timestamps["tim_start_iteration"] = tt; + } if (version >= 11) { @@ -327,6 +486,12 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve robotShape_radius = 0.5; } + if (version >= 12 && version < 15) + { + std::vector> dummy_cmd_vel_filterings; + in >> dummy_cmd_vel_filterings; + } + if (version >= 13) { in >> values >> timestamps; @@ -337,16 +502,23 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve timestamps.clear(); } - if (version >= 24) + if (version >= 14) { - in >> relPoseSense >> relPoseVelCmd; + if (version >= 24) + { + in >> relPoseSense >> relPoseVelCmd; + } + else + { + mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd; + in >> crelPoseSense >> crelPoseVelCmd; + relPoseSense = crelPoseSense.asTPose(); + relPoseVelCmd = crelPoseVelCmd.asTPose(); + } } else { - mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd; - in >> crelPoseSense >> crelPoseVelCmd; - relPoseSense = crelPoseSense.asTPose(); - relPoseVelCmd = crelPoseVelCmd.asTPose(); + relPoseSense = relPoseVelCmd = mrpt::math::TPose2D(0, 0, 0); } if (version >= 18) @@ -354,16 +526,18 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve else additional_debug_msgs.clear(); - if (version >= 24 && version < 28) - { - mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds; - for (auto& ipp : infoPerPTG) ipp.dynState = ds; - } - else if (version < 24) + if (version < 28) { - mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ds; - ds.curVelLocal = cur_vel_local; - if (!WS_targets_relative.empty()) ds.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; } From 453aa695686cc6a8ff86b5663ac2af83cfe4ddbb Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 4 Oct 2024 22:13:42 +0200 Subject: [PATCH 4/4] Fix leftover --- .../ReactiveNavigationDemo/reactive_navigator_demoMain.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp b/apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp index 7dbdfcb6f9..4bda3f6469 100644 --- a/apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp +++ b/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));