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