29 robotPoseLocalization(0, 0, 0),
30 robotPoseOdometry(0, 0, 0),
31 relPoseSense(0, 0, 0),
32 relPoseVelCmd(0, 0, 0),
33 WS_targets_relative(),
35 cur_vel_local(0, 0, 0),
36 robotShape_radius(.0),
39 rel_cur_pose_wrt_last_vel_cmd_NOP(0, 0, 0),
40 rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0)
61 for (i = 0; i <
n; i++)
69 (
const void*)&(*
infoPerPTG[i].TP_Obstacles.begin()),
75 out <<
infoPerPTG[i].timeForTPObsTransformation
85 const bool there_is_ptg_data =
infoPerPTG[i].ptg ? true :
false;
86 out << there_is_ptg_data;
87 if (there_is_ptg_data) out <<
infoPerPTG[i].ptg;
192 for (i = 0; i <
n; i++)
201 ipp.TP_Obstacles.resize(m);
203 in.ReadBufferFixEndianness(&(*ipp.TP_Obstacles.begin()), m);
205 ipp.TP_Targets.clear();
210 in >> ipp.TP_Targets;
216 ipp.TP_Targets.push_back(trg);
232 in >> ipp.timeForTPObsTransformation >>
233 ipp.timeForHolonomicMethod;
234 in >> ipp.desiredDirection >> ipp.desiredSpeed >>
239 in.ReadAsAndCastTo<float,
double>(
240 ipp.timeForTPObsTransformation);
241 in.ReadAsAndCastTo<float,
double>(
242 ipp.timeForHolonomicMethod);
243 in.ReadAsAndCastTo<float,
double>(ipp.desiredDirection);
244 in.ReadAsAndCastTo<float,
double>(ipp.desiredSpeed);
245 in.ReadAsAndCastTo<float,
double>(ipp.evaluation);
247 if (version >= 21 && version < 23)
249 double evaluation_org, evaluation_priority;
250 in >> evaluation_org >> evaluation_priority;
259 bool there_is_ptg_data;
260 in >> there_is_ptg_data;
261 if (there_is_ptg_data)
262 ipp.ptg = std::dynamic_pointer_cast<
270 std::vector<std::map<double, double>> raw_clearances;
271 in >> raw_clearances;
272 ipp.clearance.resize(
273 raw_clearances.size(), raw_clearances.size());
274 for (
size_t i = 0; i < raw_clearances.size(); i++)
275 ipp.clearance.get_path_clearance_decimated(i) =
280 ipp.clearance.readFromStream(
in);
285 ipp.clearance.clear();
302 in >> robotOdometryPose;
343 crel_pose_PTG_origin_wrt_sense_NOP;
344 in >> crel_cur_pose_wrt_last_vel_cmd_NOP >>
345 crel_pose_PTG_origin_wrt_sense_NOP;
347 crel_cur_pose_wrt_last_vel_cmd_NOP;
349 crel_pose_PTG_origin_wrt_sense_NOP;
356 if (version >= 17 && version < 24)
373 std::vector<double> vel;
381 for (
size_t i = 0; i <
cmd_vel->getVelCmdLength(); i++)
382 cmd_vel->setVelCmdElement(i, vel[i]);
401 values[
"executionTime"] = old_exec_time;
411 if (
n)
in.ReadBufferFixEndianness(&(*prevV.begin()),
n);
415 if (
n)
in.ReadBufferFixEndianness(&(*prevW.begin()),
n);
418 prevSelPTG.resize(
n);
419 if (
n)
in.ReadBufferFixEndianness(&(*prevSelPTG.begin()),
n);
439 float actual_v, actual_w;
440 in >> actual_v >> actual_w;
451 if (version < 13 && version > 1)
453 float old_estim_period;
454 in >> old_estim_period;
455 values[
"estimatedExecutionPeriod"] = old_estim_period;
470 for (
unsigned int j = 0; j <
n; j++)
493 for (i = 0; i < n; i++) in >> dummy;
506 in >> navigatorBehavior;
512 in >> doorCrossing_P1 >> doorCrossing_P2;
516 if (version > 6 && version < 13)
532 if (version >= 12 && version < 15)
534 std::vector<std::vector<double>> dummy_cmd_vel_filterings;
535 in >> dummy_cmd_vel_filterings;
557 in >> crelPoseSense >> crelPoseVelCmd;
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP
int32_t nSelectedPTG
The selected PTG.
std::vector< mrpt::math::TPose2D > WS_targets_relative
Relative poses (wrt to robotPoseLocalization) for.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::math::CVectorFloat robotShape_y
mrpt::math::TTwist2D cur_vel_local
The actual robot velocities in local (robot) coordinates, as read from sensors, in "m/sec" and "rad/s...
GLboolean GLenum GLenum GLvoid * values
std::map< std::string, mrpt::system::TTimeStamp > timestamps
Known values:
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense_NOP
mrpt::math::TPose2D robotPoseOdometry
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
mrpt::math::TPose2D relPoseSense
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
uint32_t nPTGs
The number of PTGS:
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
GLubyte GLubyte GLubyte GLubyte w
mrpt::math::CVectorFloat robotShape_x
The robot shape in WS.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ptg_last_navDynState
This is the base class for any user-defined PTG.
mrpt::math::TPose2D robotPoseLocalization
The robot pose (from odometry and from the localization/SLAM system).
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel
The final motion command sent to robot, in "m/sec" and "rad/sec".
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
int16_t ptg_index_NOP
Negative means no NOP mode evaluation, so the rest of "NOP variables" should be ignored.
mrpt::math::TPose2D relTarget
Current relative target location.
mrpt::aligned_containers< TInfoPerPTG >::vector_t infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
mrpt::maps::CSimplePointsMap WS_Obstacles_original
GLsizei const GLchar ** string
A class used to store a 2D point.
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState navDynState
void writeToStream(mrpt::utils::CStream &out) const
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel_original
Motion command as comes out from the PTG, before scaling speed limit filtering.
mrpt::math::TTwist2D cur_vel
The actual robot velocities in global (map) coordinates, as read from sensors, in "m/sec" and "rad/se...
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
void readFromStream(mrpt::utils::CStream &in)
mrpt::maps::CSimplePointsMap WS_Obstacles
The WS-Obstacles.
std::shared_ptr< CVehicleVelCmd > Ptr
unsigned __int32 uint32_t
Dynamic state that may affect the PTG path parameterization.
mrpt::math::TPose2D relPoseVelCmd
Kinematic model for Ackermann-like or differential-driven vehicles.
std::map< std::string, std::string > additional_debug_msgs
Additional debug traces.
double robotShape_radius
The circular robot radius.