29 robotPoseLocalization(0,0,0),
30 robotPoseOdometry(0,0,0),
33 WS_targets_relative(),
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)
76 const bool there_is_ptg_data =
infoPerPTG[i].ptg.present();
77 out << there_is_ptg_data;
78 if (there_is_ptg_data)
185 ipp.TP_Obstacles.resize(m);
186 if (m)
in.ReadBufferFixEndianness( &(*ipp.TP_Obstacles.begin()), m );
188 ipp.TP_Targets.clear();
192 in >> ipp.TP_Targets;
197 ipp.TP_Targets.push_back(trg);
211 in >> ipp.timeForTPObsTransformation >> ipp.timeForHolonomicMethod;
212 in >> ipp.desiredDirection >> ipp.desiredSpeed >> ipp.evaluation;
214 in.ReadAsAndCastTo<float,
double>(ipp.timeForTPObsTransformation);
215 in.ReadAsAndCastTo<float,
double>(ipp.timeForHolonomicMethod);
216 in.ReadAsAndCastTo<float,
double>(ipp.desiredDirection);
217 in.ReadAsAndCastTo<float,
double>(ipp.desiredSpeed);
218 in.ReadAsAndCastTo<float,
double>(ipp.evaluation);
221 double evaluation_org, evaluation_priority;
222 in >> evaluation_org >> evaluation_priority;
231 bool there_is_ptg_data;
232 in >> there_is_ptg_data;
233 if (there_is_ptg_data)
234 ipp.ptg = mrpt::nav::CParameterizedTrajectoryGeneratorPtr(
in.ReadObject() );
240 std::vector<std::map<double,double> > raw_clearances;
241 in >> raw_clearances;
242 ipp.clearance.resize(raw_clearances.size(), raw_clearances.size());
243 for (
size_t i = 0; i < raw_clearances.size(); i++)
244 ipp.clearance.get_path_clearance_decimated(i) = raw_clearances[i];
247 ipp.clearance.readFromStream(
in);
251 ipp.clearance.clear();
265 in >> robotOdometryPose;
303 in >> crel_cur_pose_wrt_last_vel_cmd_NOP >> crel_pose_PTG_origin_wrt_sense_NOP;
324 std::vector<double> vel;
329 for (
size_t i = 0; i <
cmd_vel->getVelCmdLength(); i++)
330 cmd_vel->setVelCmdElement(i, vel[i]);
345 float old_exec_time;
in >> old_exec_time;
346 values[
"executionTime"] = old_exec_time;
356 if (
n)
in.ReadBufferFixEndianness( &(*prevV.begin()),
n);
360 if (
n)
in.ReadBufferFixEndianness( &(*prevW.begin()),
n);
363 prevSelPTG.resize(
n);
364 if (
n)
in.ReadBufferFixEndianness( &(*prevSelPTG.begin()),
n);
381 float actual_v, actual_w;
382 in >> actual_v >> actual_w;
392 if (version < 13 && version>1) {
393 float old_estim_period;
in >> old_estim_period;
394 values[
"estimatedExecutionPeriod"] = old_estim_period;
407 for (
unsigned int j = 0; j <
n; j++) {
442 in >> navigatorBehavior;
448 in >> doorCrossing_P1 >> doorCrossing_P2;
464 std::vector<std::vector<double> > dummy_cmd_vel_filterings;
465 in >> dummy_cmd_vel_filterings;
482 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 extrapolated paths at two instants: time of obstacl...
mrpt::math::CVectorFloat robotShape_y
The robot shape in WS. Used by PTGs derived from mrpt::nav::CPTG_RobotShape_Polygonal.
mrpt::math::TTwist2D cur_vel_local
The actual robot velocities in local (robot) coordinates, as read from sensors, in "m/sec" and "rad/s...
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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
The robot pose (from odometry and from the localization/SLAM system).
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...
uint32_t nPTGs
The number of PTGS:
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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
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
mrpt::math::TPose2D robotPoseLocalization
#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::kinematics::CVehicleVelCmdPtr cmd_vel_original
Motion command as comes out from the PTG, before scaling speed limit filtering.
mrpt::kinematics::CVehicleVelCmdPtr cmd_vel
The final motion command sent to robot, in "m/sec" and "rad/sec".
mrpt::aligned_containers< TInfoPerPTG >::vector_t infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
mrpt::maps::CSimplePointsMap WS_Obstacles_original
The WS-Obstacles.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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::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)
mrpt::maps::CSimplePointsMap WS_Obstacles
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. Used by PTGs derived from mrpt::nav::CPTG_RobotShape_Circular.