MRPT
1.9.9
|
A PTG for circular paths ("C" type PTG in papers).
ptg-configurator
This PT generator functions are:
So, the radius of curvature of each trajectory is constant for each "alpha" value (the trajectory parameter):
from which a minimum radius of curvature can be set by selecting the appropriate values of V_MAX and W_MAX, knowning that .
Definition at line 40 of file CPTG_DiffDrive_C.h.
#include <mrpt/nav/tpspace/CPTG_DiffDrive_C.h>
Public Member Functions | |
void * | operator new (size_t size) |
void * | operator new[] (size_t size) |
void | operator delete (void *ptr) noexcept |
void | operator delete[] (void *ptr) noexcept |
void | operator delete (void *memory, void *ptr) noexcept |
void * | operator new (size_t size, const std::nothrow_t &) noexcept |
void | operator delete (void *ptr, const std::nothrow_t &) noexcept |
CPTG_DiffDrive_C () | |
CPTG_DiffDrive_C (const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) | |
virtual void | loadFromConfigFile (const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override |
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally, plus): More... | |
virtual void | saveToConfigFile (mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override |
This method saves the options to a ".ini"-like file or memory-stored string list. More... | |
std::string | getDescription () const override |
Gets a short textual description of the PTG and its parameters. More... | |
bool | inverseMap_WS2TP (double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const override |
The default implementation in this class relies on a look-up-table. More... | |
bool | PTG_IsIntoDomain (double x, double y) const override |
Returns the same than inverseMap_WS2TP() but without any additional cost. More... | |
void | ptgDiffDriveSteeringFunction (float alpha, float t, float x, float y, float phi, float &v, float &w) const override |
The main method to be implemented in derived classes: it defines the differential-driven differential equation. More... | |
void | loadDefaultParams () override |
Loads a set of default parameters; provided exclusively for the PTG-configurator tool. More... | |
double | getMax_V () const |
double | getMax_W () const |
bool | isPointInsideRobotShape (const double x, const double y) const override |
Returns true if the point lies within the robot shape. More... | |
void | add_robotShape_to_setOfLines (mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const override |
Auxiliary function for rendering. More... | |
void | updateNavDynamicState (const TNavDynamicState &newState, const bool force_update=false) |
To be invoked by the navigator before each navigation step, to let the PTG to react to changing dynamic conditions. More... | |
const TNavDynamicState & | getCurrentNavDynamicState () const |
void | initialize (const std::string &cacheFilename=std::string(), const bool verbose=true) |
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Space, inverseMap_WS2TP(), etc. More... | |
void | deinitialize () |
This must be called to de-initialize the PTG if some parameter is to be changed. More... | |
bool | isInitialized () const |
Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queried for paths, obstacles, etc. More... | |
uint16_t | getAlphaValuesCount () const |
Get the number of different, discrete paths in this family. More... | |
uint16_t | getPathCount () const |
Get the number of different, discrete paths in this family. More... | |
double | index2alpha (uint16_t k) const |
Alpha value for the discrete corresponding value. More... | |
uint16_t | alpha2index (double alpha) const |
Discrete index value for the corresponding alpha value. More... | |
double | getRefDistance () const |
void | initTPObstacles (std::vector< double > &TP_Obstacles) const |
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ranges, in "pseudometers", un-normalized). More... | |
void | initTPObstacleSingle (uint16_t k, double &TP_Obstacle_k) const |
double | getScorePriority () const |
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG. More... | |
void | setScorePriorty (double prior) |
unsigned | getClearanceStepCount () const |
void | setClearanceStepCount (const unsigned res) |
unsigned | getClearanceDecimatedPaths () const |
void | setClearanceDecimatedPaths (const unsigned num) |
virtual void | renderPathAsSimpleLine (const uint16_t k, mrpt::opengl::CSetOfLines &gl_obj, const double decimate_distance=0.1, const double max_path_distance=-1.0) const |
Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line). More... | |
bool | debugDumpInFiles (const std::string &ptg_name) const |
Dump PTG trajectories in four text files: `. More... | |
void | initClearanceDiagram (ClearanceDiagram &cd) const |
Must be called to resize a CD to its correct size, before calling updateClearance() More... | |
void | updateClearance (const double ox, const double oy, ClearanceDiagram &cd) const |
Updates the clearance diagram given one (ox,oy) obstacle point, in coordinates relative to the PTG path origin. More... | |
void | updateClearancePost (ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const |
virtual void | evalClearanceSingleObstacle (const double ox, const double oy, const uint16_t k, ClearanceDiagram::dist2clearance_t &inout_realdist2clearance, bool treat_as_obstacle=true) const |
Evals the robot clearance for each robot pose along path k , for the real distances in the key of the map<>, then keep in the map value the minimum of its current stored clearance, or the computed clearance. More... | |
virtual mxArray * | writeToMatlab () const |
Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class. More... | |
void | loadFromConfigFileName (const std::string &config_file, const std::string §ion) |
Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to load the file. More... | |
void | saveToConfigFileName (const std::string &config_file, const std::string §ion) const |
Behaves like saveToConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to save the file. More... | |
void | dumpToConsole () const |
Just like dumpToTextStream() but sending the text to the console (std::cout) More... | |
virtual void | dumpToTextStream (std::ostream &out) const |
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream. More... | |
Virtual interface of each PTG implementation | |
virtual mrpt::kinematics::CVehicleVelCmd::Ptr | directionToMotionCommand (uint16_t k) const override |
In this class, out_action_cmd contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s). More... | |
virtual mrpt::kinematics::CVehicleVelCmd::Ptr | getSupportedKinematicVelocityCommand () const override |
Returns an empty kinematic velocity command object of the type supported by this PTG. More... | |
virtual void | setRefDistance (const double refDist) override |
Launches an exception in this class: it is not allowed in numerical integration-based PTGs to change the reference distance after initialization. More... | |
size_t | getPathStepCount (uint16_t k) const override |
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory. More... | |
void | getPathPose (uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const override |
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step . More... | |
double | getPathDist (uint16_t k, uint32_t step) const override |
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step . More... | |
bool | getPathStepForDist (uint16_t k, double dist, uint32_t &out_step) const override |
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < dist More... | |
double | getPathStepDuration () const override |
Returns the duration (in seconds) of each "step". More... | |
double | getMaxLinVel () const override |
Returns the maximum linear velocity expected from this PTG [m/s]. More... | |
double | getMaxAngVel () const override |
Returns the maximum angular velocity expected from this PTG [rad/s]. More... | |
void | updateTPObstacle (double ox, double oy, std::vector< double > &tp_obstacles) const override |
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy) More... | |
void | updateTPObstacleSingle (double ox, double oy, uint16_t k, double &tp_obstacle_k) const override |
Like updateTPObstacle() but for one direction only (k ) in TP-Space. More... | |
virtual void | onNewNavDynamicState () override |
This family of PTGs ignores the dynamic states. More... | |
Robot shape | |
void | setRobotShape (const mrpt::math::CPolygon &robotShape) |
Robot shape must be set before initialization, either from ctor params or via this method. More... | |
const mrpt::math::CPolygon & | getRobotShape () const |
double | getMaxRobotRadius () const override |
Returns an approximation of the robot radius. More... | |
virtual double | evalClearanceToRobotShape (const double ox, const double oy) const override |
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center. More... | |
Virtual interface of each PTG implementation | |
virtual bool | isBijectiveAt (uint16_t k, uint32_t step) const |
Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa. More... | |
virtual bool | supportVelCmdNOP () const |
Returns true if it is possible to stop sending velocity commands to the robot and, still, the robot controller will be able to keep following the last sent trajectory ("NOP" velocity commands). More... | |
virtual bool | supportSpeedAtTarget () const |
Returns true if this PTG takes into account the desired velocity at target. More... | |
virtual double | maxTimeInVelCmdNOP (int path_k) const |
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path can be followed without re-issuing a new velcmd. More... | |
virtual double | getActualUnloopedPathLength (uint16_t k) const |
Returns the actual distance (in meters) of the path, discounting possible circular loops of the path (e.g. More... | |
virtual double | evalPathRelativePriority (uint16_t k, double target_distance) const |
Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others, if the k-th path is to be selected. More... | |
RTTI classes and functions for polymorphic hierarchies | |
mrpt::rtti::CObject::Ptr | duplicateGetSmartPtr () const |
Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer). More... | |
Static Public Member Functions | |
static void * | operator new (size_t size, void *ptr) |
static CParameterizedTrajectoryGenerator * | CreatePTG (const std::string &ptgClassName, const mrpt::config::CConfigFileBase &cfg, const std::string &sSection, const std::string &sKeyPrefix) |
The class factory for creating a PTG from a list of parameters in a section of a given config file (physical file or in memory). More... | |
static std::string & | OUTPUT_DEBUG_PATH_PREFIX () |
The path used as defaul output in, for example, debugDumpInFiles. More... | |
static double | index2alpha (uint16_t k, const unsigned int num_paths) |
static uint16_t | alpha2index (double alpha, const unsigned int num_paths) |
static PTG_collision_behavior_t & | COLLISION_BEHAVIOR () |
Defines the behavior when there is an obstacle inside the robot shape right at the beginning of a PTG trajectory. More... | |
Protected Types | |
using | TCollisionCell = std::vector< std::pair< uint16_t, float > > |
A list of all the pairs (alpha,distance) such as the robot collides at that cell. More... | |
Protected Member Functions | |
void | internal_processNewRobotShape () override |
Will be called whenever the robot shape is set / updated. More... | |
void | internal_initialize (const std::string &cacheFilename=std::string(), const bool verbose=true) override |
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Space, inverseMap_WS2TP(), etc. More... | |
void | internal_deinitialize () override |
This must be called to de-initialize the PTG if some parameter is to be changed. More... | |
void | internal_readFromStream (mrpt::serialization::CArchive &in) override |
void | internal_writeToStream (mrpt::serialization::CArchive &out) const override |
void | simulateTrajectories (float max_time, float max_dist, unsigned int max_n, float diferencial_t, float min_dist, float *out_max_acc_v=nullptr, float *out_max_acc_w=nullptr) |
Numerically solve the diferential equations to generate a family of trajectories. More... | |
bool | saveColGridsToFile (const std::string &filename, const mrpt::math::CPolygon &computed_robotShape) const |
bool | loadColGridsFromFile (const std::string &filename, const mrpt::math::CPolygon ¤t_robotShape) |
void | loadShapeFromConfigFile (const mrpt::config::CConfigFileBase &source, const std::string §ion) |
void | internal_shape_loadFromStream (mrpt::serialization::CArchive &in) |
void | internal_shape_saveToStream (mrpt::serialization::CArchive &out) const |
void | internal_TPObsDistancePostprocess (const double ox, const double oy, const double new_tp_obs_dist, double &inout_tp_obs) const |
To be called by implementors of updateTPObstacle() and updateTPObstacleSingle() to honor the user settings regarding COLLISION_BEHAVIOR. More... | |
CSerializable virtual methods | |
uint8_t | serializeGetVersion () const override |
Must return the current versioning number of the object. More... | |
void | serializeTo (mrpt::serialization::CArchive &out) const override |
Pure virtual method for writing (serializing) to an abstract archive. More... | |
void | serializeFrom (mrpt::serialization::CArchive &in, uint8_t serial_version) override |
Pure virtual method for reading (deserializing) from an abstract archive. More... | |
Static Protected Member Functions | |
static void | dumpVar_int (std::ostream &out, const char *varName, int v) |
Used to print variable info from dumpToTextStream with the macro LOADABLEOPTS_DUMP_VAR. More... | |
static void | dumpVar_float (std::ostream &out, const char *varName, float v) |
static void | dumpVar_double (std::ostream &out, const char *varName, double v) |
static void | dumpVar_bool (std::ostream &out, const char *varName, bool v) |
static void | dumpVar_string (std::ostream &out, const char *varName, const std::string &v) |
Protected Attributes | |
double | K |
A generation parameter. More... | |
double | V_MAX |
double | W_MAX |
double | turningRadiusReference |
std::vector< TCPointVector > | m_trajectory |
double | m_resolution |
double | m_stepTimeDuration |
CCollisionGrid | m_collisionGrid |
The collision grid. More... | |
mrpt::containers::CDynamicGrid< TCellForLambdaFunction > | m_lambdaFunctionOptimizer |
This grid will contain indexes data for speeding-up the default, brute-force lambda function. More... | |
mrpt::math::CPolygon | m_robotShape |
double | m_robotMaxRadius |
double | refDistance |
uint16_t | m_alphaValuesCount |
The number of discrete values for "alpha" between -PI and +PI. More... | |
double | m_score_priority |
uint16_t | m_clearance_num_points |
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0,1] (Default=5) More... | |
uint16_t | m_clearance_decimated_paths |
Number of paths for the decimated paths analysis of clearance. More... | |
TNavDynamicState | m_nav_dyn_state |
Updated before each nav step by. More... | |
uint16_t | m_nav_dyn_state_target_k |
Update in updateNavDynamicState(), contains the path index (k) for the target. More... | |
bool | m_is_initialized |
Static Protected Attributes | |
static const uint16_t | INVALID_PTG_PATH_INDEX = static_cast<uint16_t>(-1) |
RTTI stuff | |
using | Ptr = std::shared_ptr< CPTG_DiffDrive_C > |
using | ConstPtr = std::shared_ptr< const CPTG_DiffDrive_C > |
using | UniquePtr = std::unique_ptr< CPTG_DiffDrive_C > |
using | ConstUniquePtr = std::unique_ptr< const CPTG_DiffDrive_C > |
static mrpt::rtti::CLASSINIT | _init_CPTG_DiffDrive_C |
static const mrpt::rtti::TRuntimeClassId | runtimeClassId |
static constexpr const char * | className = "CPTG_DiffDrive_C" |
static const mrpt::rtti::TRuntimeClassId * | _GetBaseClass () |
static constexpr auto | getClassName () |
static const mrpt::rtti::TRuntimeClassId & | GetRuntimeClassIdStatic () |
static mrpt::rtti::CObject * | CreateObject () |
template<typename... Args> | |
static Ptr | Create (Args &&... args) |
template<typename... Args> | |
static UniquePtr | CreateUnique (Args &&... args) |
virtual const mrpt::rtti::TRuntimeClassId * | GetRuntimeClass () const override |
Returns information about the class of an object in runtime. More... | |
virtual mrpt::rtti::CObject * | clone () const override |
Returns a deep copy (clone) of the object, indepently of its class. More... | |
using mrpt::nav::CPTG_DiffDrive_C::ConstPtr = std::shared_ptr<const CPTG_DiffDrive_C > |
Definition at line 42 of file CPTG_DiffDrive_C.h.
using mrpt::nav::CPTG_DiffDrive_C::ConstUniquePtr = std::unique_ptr<const CPTG_DiffDrive_C > |
Definition at line 42 of file CPTG_DiffDrive_C.h.
A type for the associated smart pointer
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
protectedinherited |
A list of all the pairs (alpha,distance) such as the robot collides at that cell.
Definition at line 165 of file CPTG_DiffDrive_CollisionGridBased.h.
using mrpt::nav::CPTG_DiffDrive_C::UniquePtr = std::unique_ptr< CPTG_DiffDrive_C > |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
inline |
Definition at line 44 of file CPTG_DiffDrive_C.h.
|
inline |
Definition at line 45 of file CPTG_DiffDrive_C.h.
References loadFromConfigFile().
|
staticprotected |
|
overridevirtualinherited |
Auxiliary function for rendering.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 96 of file CPTG_RobotShape_Polygonal.cpp.
References mrpt::opengl::CSetOfLines::appendLine(), mrpt::opengl::CSetOfLines::appendLineStrip(), mrpt::poses::CPose2D::composePoint(), and mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape.
|
inherited |
Discrete index value for the corresponding alpha value.
Definition at line 228 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), mrpt::nav::CHolonomicFullEval::evalSingleTarget(), mrpt::nav::CAbstractPTGBasedReactive::generate_vel_cmd(), mrpt::nav::CPTG_Holo_Blend::inverseMap_WS2TP(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), and mrpt::nav::ClearanceDiagram::renderAs3DObject().
|
staticinherited |
Definition at line 218 of file CParameterizedTrajectoryGenerator.cpp.
References M_PI, mrpt::round(), and mrpt::math::wrapToPi().
|
overridevirtual |
Returns a deep copy (clone) of the object, indepently of its class.
Implements mrpt::rtti::CObject.
|
staticinherited |
Defines the behavior when there is an obstacle inside the robot shape right at the beginning of a PTG trajectory.
Default value: COLL_BEH_BACK_AWAY
Definition at line 30 of file CParameterizedTrajectoryGenerator.cpp.
References COLLISION_BEHAVIOR.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::internal_TPObsDistancePostprocess().
|
inlinestatic |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
static |
|
staticinherited |
The class factory for creating a PTG from a list of parameters in a section of a given config file (physical file or in memory).
Possible parameters are:
ptgClassName
can be any PTG class name which has been registered as any other mrpt::serialization::CSerializable class.
std::logic_error | On invalid or missing parameters. |
Definition at line 21 of file CParameterizedTrajectoryGenerator_factory.cpp.
References mrpt::config::CConfigFilePrefixer::bind(), mrpt::rtti::TRuntimeClassId::createObject(), mrpt::rtti::findRegisteredClass(), mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile(), MRPT_END, MRPT_START, mrpt::rtti::registerAllPendingClasses(), mrpt::config::CConfigFilePrefixer::setPrefixes(), THROW_EXCEPTION_FMT, and mrpt::system::trim().
Referenced by mrpt::nav::PlannerTPS_VirtualBase::internal_loadConfig_PTG(), mrpt::nav::CReactiveNavigationSystem::loadConfigFile(), and mrpt::nav::CReactiveNavigationSystem3D::loadConfigFile().
|
inlinestatic |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
inherited |
Dump PTG trajectories in four text files: `.
/reactivenav.logs/PTGs/PTGi_{x,y,phi,d}.txt` Text files are loadable from MATLAB/Octave, and can be visualized with the script [MRPT_DIR]/scripts/viewPTG.m
Definition at line 284 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::system::createDirectory(), mrpt::format(), mrpt::nav::CParameterizedTrajectoryGenerator::getAlphaValuesCount(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepCount(), min, and mrpt::nav::CParameterizedTrajectoryGenerator::OUTPUT_DEBUG_PATH_PREFIX().
|
inherited |
This must be called to de-initialize the PTG if some parameter is to be changed.
After changing it, call initialize again
Definition at line 421 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::CParameterizedTrajectoryGenerator::internal_deinitialize(), and mrpt::nav::CParameterizedTrajectoryGenerator::m_is_initialized.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream().
|
overridevirtualinherited |
In this class, out_action_cmd
contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s).
In this class, out_action_cmd
contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s)
See more docs in CParameterizedTrajectoryGenerator::directionToMotionCommand()
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 276 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References mrpt::kinematics::CVehicleVelCmd_DiffDriven::ang_vel, mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha(), mrpt::kinematics::CVehicleVelCmd_DiffDriven::lin_vel, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::ptgDiffDriveSteeringFunction().
|
inherited |
Just like dumpToTextStream() but sending the text to the console (std::cout)
Definition at line 44 of file CLoadableOptions.cpp.
References mrpt::config::CLoadableOptions::dumpToTextStream().
Referenced by mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel().
|
virtualinherited |
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
The default implementation in this base class relies on saveToConfigFile() to generate a plain text representation of all the parameters.
Reimplemented in mrpt::vision::TMultiResDescOptions, mrpt::vision::TMultiResDescMatchOptions, mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions, mrpt::maps::COccupancyGridMap2D::TInsertionOptions, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TLoopClosureParams, mrpt::vision::TMatchingOptions, mrpt::hmtslam::CHMTSLAM::TOptions, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TLaserParams, mrpt::maps::CLandmarksMap::TLikelihoodOptions, mrpt::maps::TSetOfMetricMapInitializers, mrpt::maps::CPointsMap::TRenderOptions, mrpt::maps::CPointsMap::TLikelihoodOptions, mrpt::vision::TStereoSystemParams, mrpt::maps::CColouredPointsMap::TColourOptions, mrpt::maps::CLandmarksMap::TInsertionOptions, mrpt::maps::COctoMapBase< octree_t, octree_node_t >::TLikelihoodOptions, mrpt::maps::CPointsMap::TInsertionOptions, mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::GraphVisualizationParams, mrpt::slam::CRangeBearingKFSLAM::TOptions, mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::OptimizationParams, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::TParams, mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::TParams, mrpt::graphslam::deciders::CRangeScanOps< GRAPH_T >::TParams, mrpt::maps::CRandomFieldGridMap3D::TInsertionOptions, mrpt::maps::CBeaconMap::TInsertionOptions, mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::TParams, mrpt::maps::CMultiMetricMapPDF::TPredictionParams, mrpt::maps::CBeaconMap::TLikelihoodOptions, mrpt::slam::CRangeBearingKFSLAM2D::TOptions, mrpt::maps::CHeightGridMap2D::TInsertionOptions, mrpt::vision::CFeatureExtraction::TOptions, mrpt::maps::COctoMapBase< octree_t, octree_node_t >::TInsertionOptions, mrpt::graphslam::deciders::CIncrementalNodeRegistrationDecider< GRAPH_T >::TParams, mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions, mrpt::bayes::TKF_options, mrpt::slam::CGridMapAligner::TConfigParams, mrpt::graphslam::TSlidingWindow, mrpt::maps::CReflectivityGridMap2D::TInsertionOptions, mrpt::graphslam::TUncertaintyPath< GRAPH_T >, mrpt::hmtslam::CTopLCDetector_GridMatching::TOptions, mrpt::hmtslam::CTopLCDetector_FabMap::TOptions, mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions, mrpt::maps::CGasConcentrationGridMap2D::TInsertionOptions, mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions, mrpt::maps::TMetricMapInitializer, mrpt::slam::CMetricMapBuilderICP::TConfigParams, mrpt::vision::CCamModel, and mrpt::slam::TKLDParams.
Definition at line 77 of file CLoadableOptions.cpp.
References mrpt::config::CConfigFileMemory::getContent(), and mrpt::config::CLoadableOptions::saveToConfigFile().
Referenced by mrpt::config::CLoadableOptions::dumpToConsole(), and mrpt::maps::TMetricMapInitializer::dumpToTextStream().
|
staticprotectedinherited |
Definition at line 63 of file CLoadableOptions.cpp.
References mrpt::format(), and LOADABLEOPTS_COLUMN_WIDTH.
|
staticprotectedinherited |
Definition at line 57 of file CLoadableOptions.cpp.
References mrpt::format(), and LOADABLEOPTS_COLUMN_WIDTH.
|
staticprotectedinherited |
Definition at line 51 of file CLoadableOptions.cpp.
References mrpt::format(), and LOADABLEOPTS_COLUMN_WIDTH.
|
staticprotectedinherited |
Used to print variable info from dumpToTextStream with the macro LOADABLEOPTS_DUMP_VAR.
Definition at line 45 of file CLoadableOptions.cpp.
References mrpt::format(), and LOADABLEOPTS_COLUMN_WIDTH.
|
staticprotectedinherited |
Definition at line 70 of file CLoadableOptions.cpp.
References mrpt::format(), and LOADABLEOPTS_COLUMN_WIDTH.
|
inlineinherited |
Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer).
Definition at line 168 of file CObject.h.
References mrpt::rtti::CObject::clone().
Referenced by mrpt::obs::CRawlog::addActions(), and mrpt::obs::CRawlog::addObservations().
|
virtualinherited |
Evals the robot clearance for each robot pose along path k
, for the real distances in the key of the map<>, then keep in the map value the minimum of its current stored clearance, or the computed clearance.
In case of collision, clearance is zero.
treat_as_obstacle | true: normal use for obstacles; false: compute shortest distances to a target point (no collision) |
Definition at line 518 of file CParameterizedTrajectoryGenerator.cpp.
References ASSERT_, mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceToRobotShape(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepCount(), mrpt::math::TPose2D::inverseComposePoint(), mrpt::keep_min(), mrpt::math::TPoint2D::norm(), mrpt::nav::CParameterizedTrajectoryGenerator::refDistance, mrpt::round(), mrpt::math::TPoint2D::x, and mrpt::math::TPoint2D::y.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateClearance().
|
overridevirtualinherited |
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center.
Zero or negative means collision.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 158 of file CPTG_RobotShape_Polygonal.cpp.
References mrpt::hypot_fast(), mrpt::nav::CPTG_RobotShape_Polygonal::isPointInsideRobotShape(), mrpt::keep_max(), and mrpt::nav::CPTG_RobotShape_Polygonal::m_robotMaxRadius.
|
inlinevirtualinherited |
Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others, if the k-th path is to be selected.
Definition at line 295 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores().
|
inlinevirtualinherited |
Returns the actual distance (in meters) of the path, discounting possible circular loops of the path (e.g.
if it comes back to the origin). Default: refDistance
Definition at line 288 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::refDistance.
|
inlineinherited |
Get the number of different, discrete paths in this family.
Definition at line 341 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CParameterizedTrajectoryGenerator::debugDumpInFiles(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize().
|
inlinestatic |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
inlineinherited |
Definition at line 370 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_decimated_paths.
|
inlineinherited |
Definition at line 364 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_num_points.
|
inlineinherited |
Definition at line 319 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state.
|
overridevirtual |
Gets a short textual description of the PTG and its parameters.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 65 of file CPTG_DiffDrive_C.cpp.
References mrpt::format().
|
inlineinherited |
Definition at line 112 of file CPTG_DiffDrive_CollisionGridBased.h.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::V_MAX.
|
inlineinherited |
Definition at line 113 of file CPTG_DiffDrive_CollisionGridBased.h.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::W_MAX.
|
inlineoverridevirtualinherited |
Returns the maximum angular velocity expected from this PTG [rad/s].
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 98 of file CPTG_DiffDrive_CollisionGridBased.h.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::W_MAX.
|
inlineoverridevirtualinherited |
Returns the maximum linear velocity expected from this PTG [m/s].
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 97 of file CPTG_DiffDrive_CollisionGridBased.h.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::V_MAX.
|
overridevirtualinherited |
Returns an approximation of the robot radius.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 147 of file CPTG_RobotShape_Polygonal.cpp.
References mrpt::nav::CPTG_RobotShape_Polygonal::m_robotMaxRadius.
|
inlineinherited |
Get the number of different, discrete paths in this family.
Definition at line 343 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::impl_waypoint_is_reachable(), and TEST().
|
overridevirtualinherited |
Access path k
([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step
.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 858 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERT_, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize().
|
overridevirtualinherited |
Access path k
([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step
.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 847 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERT_, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize().
|
overridevirtualinherited |
Access path k
([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
May be actual steps from a numerical integration or an arbitrary small length for analytical PTGs.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 840 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERT_, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize().
|
overridevirtualinherited |
Returns the duration (in seconds) of each "step".
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 956 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_stepTimeDuration.
|
overridevirtualinherited |
Access path k
([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < dist
[in] | dist | Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist]) |
k
(e.g. out of reference distance). Note that, anyway, the maximum distance (closest point) is returned in out_step
. Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 867 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERT_, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
|
inlineinherited |
Definition at line 353 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::refDistance.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), and TEST().
|
inlineinherited |
Definition at line 516 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape.
|
overridevirtual |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::nav::CParameterizedTrajectoryGenerator.
|
static |
|
inlineinherited |
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG.
Assign values <1 to PTGs with low priority.
Definition at line 362 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_score_priority.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores().
|
overridevirtualinherited |
Returns an empty kinematic velocity command object of the type supported by this PTG.
Can be queried to determine the expected kinematic interface of the PTG.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 949 of file CPTG_DiffDrive_CollisionGridBased.cpp.
|
inherited |
Alpha value for the discrete corresponding value.
Definition at line 213 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), mrpt::nav::CPTG_Holo_Blend::directionToMotionCommand(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::directionToMotionCommand(), mrpt::nav::CPTG_Holo_Blend::getPathDist(), mrpt::nav::CPTG_Holo_Blend::getPathPose(), mrpt::nav::CPTG_Holo_Blend::getPathStepForDist(), mrpt::nav::CHolonomicFullEval::navigate(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories(), and mrpt::nav::CPTG_Holo_Blend::updateTPObstacleSingle().
|
staticinherited |
Definition at line 206 of file CParameterizedTrajectoryGenerator.cpp.
References ASSERT_BELOW_, and M_PI.
|
inherited |
Must be called to resize a CD to its correct size, before calling updateClearance()
Definition at line 471 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::ClearanceDiagram::decimated_k_to_real_k(), mrpt::nav::ClearanceDiagram::get_path_clearance_decimated(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepCount(), mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount, mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_decimated_paths, mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_num_points, mrpt::nav::ClearanceDiagram::resize(), and mrpt::round().
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate().
|
inherited |
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Space, inverseMap_WS2TP(), etc.
Definition at line 406 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::system::fileNameStripInvalidChars(), mrpt::nav::CParameterizedTrajectoryGenerator::getDescription(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_initialize(), and mrpt::nav::CParameterizedTrajectoryGenerator::m_is_initialized.
|
inherited |
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ranges, in "pseudometers", un-normalized).
Definition at line 268 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle(), and mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::PlannerTPS_VirtualBase::spaceTransformer(), and TEST().
|
inherited |
Definition at line 275 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepCount(), mrpt::nav::CParameterizedTrajectoryGenerator::INVALID_PTG_PATH_INDEX, mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state_target_k, min, and mrpt::nav::CParameterizedTrajectoryGenerator::refDistance.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles(), and mrpt::nav::PlannerTPS_VirtualBase::spaceTransformerOneDirectionOnly().
|
overrideprotectedvirtualinherited |
This must be called to de-initialize the PTG if some parameter is to be changed.
After changing it, call initialize again
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 683 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories().
|
overrideprotectedvirtualinherited |
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Space, inverseMap_WS2TP(), etc.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 688 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERT_, ASSERTMSG_, mrpt::math::TPolygon2D::contains(), mrpt::format(), mrpt::nav::CParameterizedTrajectoryGenerator::getAlphaValuesCount(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathDist(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathPose(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathStepCount(), mrpt::containers::CDynamicGrid< T >::getResolution(), mrpt::containers::CDynamicGrid< T >::getSizeX(), mrpt::containers::CDynamicGrid< T >::getSizeY(), mrpt::math::CPolygon::GetVertex_x(), mrpt::math::CPolygon::GetVertex_y(), mrpt::containers::CDynamicGrid< T >::idx2x(), mrpt::containers::CDynamicGrid< T >::idx2y(), mrpt::keep_max(), mrpt::keep_min(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadColGridsFromFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_collisionGrid, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_resolution, mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape, min, MRPT_END, MRPT_START, mrpt::nav::CParameterizedTrajectoryGenerator::refDistance, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveColGridsToFile(), mrpt::containers::CDynamicGrid< T >::setSize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories(), mrpt::system::CTicTac::Tac(), mrpt::system::CTicTac::Tic(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::updateCellInfo(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::V_MAX, mrpt::math::CPolygon::verticesCount(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::W_MAX, mrpt::math::TPoint2D::x, mrpt::containers::CDynamicGrid< T >::x2idx(), mrpt::math::TPoint2D::y, and mrpt::containers::CDynamicGrid< T >::y2idx().
|
overrideprotectedvirtualinherited |
Will be called whenever the robot shape is set / updated.
Implements mrpt::nav::CPTG_RobotShape_Polygonal.
Definition at line 676 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERTMSG_, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
|
overrideprotectedvirtualinherited |
Reimplemented from mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 915 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_deinitialize(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream(), mrpt::nav::CPTG_RobotShape_Polygonal::internal_shape_loadFromStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_resolution, mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory, MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::turningRadiusReference, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::V_MAX, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::W_MAX.
Referenced by mrpt::nav::CPTG_DiffDrive_CC::serializeFrom(), mrpt::nav::CPTG_DiffDrive_CS::serializeFrom(), mrpt::nav::CPTG_DiffDrive_CCS::serializeFrom(), mrpt::nav::CPTG_DiffDrive_alpha::serializeFrom(), and serializeFrom().
|
protectedinherited |
Definition at line 122 of file CPTG_RobotShape_Polygonal.cpp.
References mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape, and MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream().
|
protectedinherited |
Definition at line 138 of file CPTG_RobotShape_Polygonal.cpp.
References mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream().
|
protectedinherited |
To be called by implementors of updateTPObstacle() and updateTPObstacleSingle() to honor the user settings regarding COLLISION_BEHAVIOR.
new_tp_obs_dist | The newly determiend collision-free ranges, in "pseudometers", un-normalized, for some "k" direction. |
inout_tp_obs | The target where to store the new TP-Obs distance, if it fulfills the criteria determined by the collision behavior. |
Definition at line 428 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::COLL_BEH_BACK_AWAY, mrpt::nav::COLL_BEH_STOP, mrpt::nav::CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR(), mrpt::nav::CParameterizedTrajectoryGenerator::getMaxRobotRadius(), mrpt::nav::CParameterizedTrajectoryGenerator::isPointInsideRobotShape(), mrpt::keep_min(), and THROW_EXCEPTION.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::updateTPObstacle(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::updateTPObstacleSingle().
|
overrideprotectedvirtualinherited |
Reimplemented from mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 935 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References mrpt::nav::CPTG_RobotShape_Polygonal::internal_shape_saveToStream(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_resolution, mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::turningRadiusReference, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::V_MAX, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::W_MAX.
Referenced by mrpt::nav::CPTG_DiffDrive_CC::serializeTo(), mrpt::nav::CPTG_DiffDrive_CS::serializeTo(), mrpt::nav::CPTG_DiffDrive_CCS::serializeTo(), mrpt::nav::CPTG_DiffDrive_alpha::serializeTo(), and serializeTo().
|
overridevirtual |
The default implementation in this class relies on a look-up-table.
Derived classes may redefine this to closed-form expressions, when they exist. See full docs in base class CParameterizedTrajectoryGenerator::inverseMap_WS2TP()
Reimplemented from mrpt::nav::CPTG_DiffDrive_CollisionGridBased.
Definition at line 90 of file CPTG_DiffDrive_C.cpp.
References ASSERT_ABOVEEQ_, ASSERT_BELOW_, M_PI, MRPT_UNUSED_PARAM, R, mrpt::sign(), and mrpt::math::wrapTo2PiInPlace().
|
inlinevirtualinherited |
Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa.
Default implementation returns true
.
Definition at line 152 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores().
|
inherited |
Returns true if initialize()
has been called and there was no errors, so the PTG is ready to be queried for paths, obstacles, etc.
Definition at line 360 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_is_initialized.
|
overridevirtualinherited |
Returns true if the point lies within the robot shape.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 152 of file CPTG_RobotShape_Polygonal.cpp.
References mrpt::math::TPolygon2D::contains(), and mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape.
Referenced by mrpt::nav::CPTG_RobotShape_Polygonal::evalClearanceToRobotShape().
|
protectedinherited |
Definition at line 358 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References mrpt::serialization::archiveFrom(), mrpt::io::CFileGZInputStream::fileOpenCorrectly(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::loadFromFile(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_collisionGrid.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize().
|
overridevirtual |
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Reimplemented from mrpt::nav::CPTG_DiffDrive_CollisionGridBased.
Definition at line 158 of file CPTG_DiffDrive_C.cpp.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadDefaultParams().
|
overridevirtual |
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally, plus):
${sKeyPrefix}resolution
: The cell size${sKeyPrefix}v_max
, ${sKeyPrefix}w_max`: Maximum robot speeds.
`${sKeyPrefix}shape_x{0,1,2..}`,
${sKeyPrefix}shape_y{0,1,2..}`: Polygonal robot shape [Optional, can be also set via setRobotPolygonShape()
]See docs of derived classes for additional parameters in setParams()
Reimplemented from mrpt::nav::CPTG_DiffDrive_CollisionGridBased.
Referenced by CPTG_DiffDrive_C().
|
inherited |
Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to load the file.
Definition at line 22 of file CLoadableOptions.cpp.
References mrpt::config::CLoadableOptions::loadFromConfigFile().
|
protectedinherited |
Definition at line 46 of file CPTG_RobotShape_Polygonal.cpp.
References mrpt::math::CPolygon::AddVertex(), ASSERTMSG_, mrpt::format(), mrpt::nav::CPTG_RobotShape_Polygonal::internal_processNewRobotShape(), mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape, and mrpt::config::CConfigFileBase::read_double().
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile().
|
virtualinherited |
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path can be followed without re-issuing a new velcmd.
Note that this is only an absolute maximum duration, navigation implementations will check for many other conditions. Default method in the base virtual class returns 0.
path_k | Queried path k index [0,N-1] |
Reimplemented in mrpt::nav::CPTG_Holo_Blend.
Definition at line 63 of file CParameterizedTrajectoryGenerator.cpp.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), and mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().
|
inlineoverridevirtualinherited |
This family of PTGs ignores the dynamic states.
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 105 of file CPTG_DiffDrive_CollisionGridBased.h.
|
inlinenoexcept |
Definition at line 42 of file CPTG_DiffDrive_C.h.
Definition at line 42 of file CPTG_DiffDrive_C.h.
Definition at line 42 of file CPTG_DiffDrive_C.h.
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
inline |
Definition at line 42 of file CPTG_DiffDrive_C.h.
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
inlinenoexcept |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
inline |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
staticinherited |
The path used as defaul output in, for example, debugDumpInFiles.
(Default="./reactivenav.logs/")
Definition at line 24 of file CParameterizedTrajectoryGenerator.cpp.
References OUTPUT_DEBUG_PATH_PREFIX.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::debugDumpInFiles().
|
overridevirtual |
Returns the same than inverseMap_WS2TP() but without any additional cost.
The default implementation just calls inverseMap_WS2TP() and discards (k,d).
Reimplemented from mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 83 of file CPTG_DiffDrive_C.cpp.
References MRPT_UNUSED_PARAM.
|
overridevirtual |
The main method to be implemented in derived classes: it defines the differential-driven differential equation.
Implements mrpt::nav::CPTG_DiffDrive_CollisionGridBased.
Definition at line 70 of file CPTG_DiffDrive_C.cpp.
References M_PI, MRPT_UNUSED_PARAM, and mrpt::sign().
|
virtualinherited |
Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line).
[in] | k | The 0-based index of the selected trajectory (discrete "alpha" parameter). |
[out] | gl_obj | Output object. |
[in] | decimate_distance | Minimum distance between path points (in meters). |
[in] | max_path_distance | If >=0, cut the path at this distance (in meters). |
Definition at line 233 of file CParameterizedTrajectoryGenerator.cpp.
References mrpt::opengl::CSetOfLines::appendLine(), mrpt::opengl::CSetOfLines::appendLineStrip(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose(), and mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepCount().
Referenced by mrpt::nav::PlannerTPS_VirtualBase::renderMoveTree().
|
protectedinherited |
Definition at line 335 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References mrpt::serialization::archiveFrom(), mrpt::io::CFileGZOutputStream::fileOpenCorrectly(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_collisionGrid, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::saveToFile().
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize().
|
overridevirtual |
This method saves the options to a ".ini"-like file or memory-stored string list.
Reimplemented from mrpt::nav::CPTG_DiffDrive_CollisionGridBased.
Definition at line 29 of file CPTG_DiffDrive_C.cpp.
References MRPT_END, MRPT_START, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(), and mrpt::config::CConfigFileBase::write().
|
inherited |
Behaves like saveToConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to save the file.
Definition at line 37 of file CLoadableOptions.cpp.
References mrpt::config::CLoadableOptions::saveToConfigFile().
|
overrideprotectedvirtual |
Pure virtual method for reading (deserializing) from an abstract archive.
Users don't call this method directly. Instead, use stream >> object;
.
in | The input binary stream where the object data must read from. |
version | The version of the object stored in the stream: use this version number in your code to know how to read the incoming data. |
std::exception | On any I/O error |
Implements mrpt::serialization::CSerializable.
Definition at line 43 of file CPTG_DiffDrive_C.cpp.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), and MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION.
|
overrideprotectedvirtual |
Must return the current versioning number of the object.
Start in zero for new classes, and increments each time there is a change in the stored format.
Implements mrpt::serialization::CSerializable.
Definition at line 58 of file CPTG_DiffDrive_C.cpp.
|
overrideprotectedvirtual |
Pure virtual method for writing (serializing) to an abstract archive.
Users don't call this method directly. Instead, use stream << object;
.
std::exception | On any I/O error |
Implements mrpt::serialization::CSerializable.
Definition at line 59 of file CPTG_DiffDrive_C.cpp.
References mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream().
|
inlineinherited |
Definition at line 374 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_decimated_paths.
|
inlineinherited |
Definition at line 365 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_num_points.
|
overridevirtualinherited |
Launches an exception in this class: it is not allowed in numerical integration-based PTGs to change the reference distance after initialization.
Reimplemented from mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 667 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERTMSG_, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory, and mrpt::nav::CParameterizedTrajectoryGenerator::refDistance.
|
inherited |
Robot shape must be set before initialization, either from ctor params or via this method.
Definition at line 24 of file CPTG_RobotShape_Polygonal.cpp.
References ASSERT_ABOVEEQ_, mrpt::nav::CPTG_RobotShape_Polygonal::internal_processNewRobotShape(), mrpt::keep_max(), mrpt::nav::CPTG_RobotShape_Polygonal::m_robotMaxRadius, and mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape.
Referenced by mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), and mrpt::nav::CReactiveNavigationSystem3D::STEP1_InitPTGs().
|
inlineinherited |
Definition at line 363 of file CParameterizedTrajectoryGenerator.h.
References mrpt::nav::CParameterizedTrajectoryGenerator::m_score_priority.
|
protectedinherited |
Numerically solve the diferential equations to generate a family of trajectories.
Definition at line 108 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERT_, mrpt::format(), mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_deinitialize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::TCellForLambdaFunction::k_max, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::TCellForLambdaFunction::k_min, mrpt::keep_max(), mrpt::keep_min(), mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_lambdaFunctionOptimizer, M_PI, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_stepTimeDuration, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory, min, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::TCellForLambdaFunction::n_max, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::TCellForLambdaFunction::n_min, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::ptgDiffDriveSteeringFunction(), mrpt::square(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::turningRadiusReference.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize().
|
inlinevirtualinherited |
Returns true if this PTG takes into account the desired velocity at target.
Definition at line 274 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState().
|
virtualinherited |
Returns true if it is possible to stop sending velocity commands to the robot and, still, the robot controller will be able to keep following the last sent trajectory ("NOP" velocity commands).
Default implementation returns "false".
Reimplemented in mrpt::nav::CPTG_Holo_Blend.
Definition at line 59 of file CParameterizedTrajectoryGenerator.cpp.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), and mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().
|
inherited |
Updates the clearance diagram given one (ox,oy) obstacle point, in coordinates relative to the PTG path origin.
[in,out] | cd | The clearance will be updated here. |
Definition at line 494 of file CParameterizedTrajectoryGenerator.cpp.
References ASSERT_, mrpt::nav::ClearanceDiagram::decimated_k_to_real_k(), mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle(), mrpt::nav::ClearanceDiagram::get_actual_num_paths(), mrpt::nav::ClearanceDiagram::get_decimated_num_paths(), mrpt::nav::ClearanceDiagram::get_path_clearance_decimated(), mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount, and mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_num_points.
Referenced by mrpt::nav::CReactiveNavigationSystem::STEP3_WSpaceToTPSpace().
|
inherited |
Definition at line 512 of file CParameterizedTrajectoryGenerator.cpp.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate().
|
inherited |
To be invoked by the navigator before each navigation step, to let the PTG to react to changing dynamic conditions.
Definition at line 365 of file CParameterizedTrajectoryGenerator.cpp.
References ASSERT_, mrpt::nav::CParameterizedTrajectoryGenerator::INVALID_PTG_PATH_INDEX, mrpt::nav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP(), mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount, mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state, mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state_target_k, mrpt::nav::CParameterizedTrajectoryGenerator::onNewNavDynamicState(), mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::relTarget, mrpt::nav::CParameterizedTrajectoryGenerator::supportSpeedAtTarget(), mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::targetRelSpeed, mrpt::math::TPose2D::x, and mrpt::math::TPose2D::y.
Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().
|
overridevirtualinherited |
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)
[in,out] | tp_obstacles | A vector of length getAlphaValuesCount() , initialized with initTPObstacles() (collision-free ranges, in "pseudometers", un-normalized). |
[in] | ox | Obstacle point (X), relative coordinates wrt origin of the PTG. |
[in] | oy | Obstacle point (Y), relative coordinates wrt origin of the PTG. |
tp_obstacles
must be initialized with initTPObstacle() before call. Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 888 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERTMSG_, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::getTPObstacle(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_TPObsDistancePostprocess(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_collisionGrid, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
|
overridevirtualinherited |
Like updateTPObstacle() but for one direction only (k
) in TP-Space.
tp_obstacle_k
must be initialized with initTPObstacleSingle() before call (collision-free ranges, in "pseudometers", un-normalized).
Implements mrpt::nav::CParameterizedTrajectoryGenerator.
Definition at line 901 of file CPTG_DiffDrive_CollisionGridBased.cpp.
References ASSERTMSG_, mrpt::nav::CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::getTPObstacle(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_TPObsDistancePostprocess(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_collisionGrid, and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::m_trajectory.
|
inlinevirtualinherited |
Introduces a pure virtual method responsible for writing to a mxArray
Matlab object, typically a MATLAB struct
whose contents are documented in each derived class.
mxArray
(caller is responsible of memory freeing) or nullptr is class does not support conversion to MATLAB. Definition at line 68 of file CSerializable.h.
|
staticprotected |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
static |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
staticprotectedinherited |
Definition at line 462 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState().
|
protected |
A generation parameter.
Definition at line 69 of file CPTG_DiffDrive_C.h.
|
protectedinherited |
The number of discrete values for "alpha" between -PI and +PI.
Definition at line 449 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index(), mrpt::nav::CParameterizedTrajectoryGenerator::getAlphaValuesCount(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathCount(), mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha(), mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles(), mrpt::nav::CPTG_Holo_Blend::internal_initialize(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(), mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams(), mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile(), mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories(), mrpt::nav::CParameterizedTrajectoryGenerator::updateClearance(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState().
|
protectedinherited |
Number of paths for the decimated paths analysis of clearance.
Definition at line 455 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::getClearanceDecimatedPaths(), mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream(), mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams(), mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile(), mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile(), and mrpt::nav::CParameterizedTrajectoryGenerator::setClearanceDecimatedPaths().
|
protectedinherited |
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0,1] (Default=5)
Definition at line 453 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::getClearanceStepCount(), mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream(), mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams(), mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile(), mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile(), mrpt::nav::CParameterizedTrajectoryGenerator::setClearanceStepCount(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateClearance().
|
protectedinherited |
The collision grid.
Definition at line 218 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadColGridsFromFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveColGridsToFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::updateTPObstacle(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::updateTPObstacleSingle().
|
protectedinherited |
|
protectedinherited |
This grid will contain indexes data for speeding-up the default, brute-force lambda function.
Definition at line 245 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories().
|
protectedinherited |
Updated before each nav step by.
Definition at line 457 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::getCurrentNavDynamicState(), mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile(), mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState().
|
protectedinherited |
Update in updateNavDynamicState(), contains the path index (k) for the target.
Definition at line 460 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState().
|
protectedinherited |
Definition at line 145 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadDefaultParams(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::loadFromFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::saveToFile().
|
protectedinherited |
|
protectedinherited |
Definition at line 530 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CPTG_RobotShape_Polygonal::add_robotShape_to_setOfLines(), mrpt::nav::CPTG_RobotShape_Polygonal::getRobotShape(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), mrpt::nav::CPTG_RobotShape_Polygonal::internal_shape_loadFromStream(), mrpt::nav::CPTG_RobotShape_Polygonal::internal_shape_saveToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(), mrpt::nav::CPTG_RobotShape_Polygonal::isPointInsideRobotShape(), mrpt::nav::CPTG_RobotShape_Polygonal::loadDefaultParams(), mrpt::nav::CPTG_RobotShape_Polygonal::loadShapeFromConfigFile(), mrpt::nav::CPTG_RobotShape_Polygonal::saveToConfigFile(), and mrpt::nav::CPTG_RobotShape_Polygonal::setRobotShape().
|
protectedinherited |
Definition at line 450 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::getScorePriority(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream(), mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams(), mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile(), mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile(), and mrpt::nav::CParameterizedTrajectoryGenerator::setScorePriorty().
|
protectedinherited |
Definition at line 146 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathStepDuration(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories().
|
protectedinherited |
Definition at line 144 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathDist(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathPose(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathStepCount(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getPathStepForDist(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_deinitialize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_processNewRobotShape(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::setRefDistance(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::updateTPObstacle(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::updateTPObstacleSingle().
|
protectedinherited |
Definition at line 447 of file CParameterizedTrajectoryGenerator.h.
Referenced by mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle(), mrpt::nav::CParameterizedTrajectoryGenerator::getActualUnloopedPathLength(), mrpt::nav::CParameterizedTrajectoryGenerator::getRefDistance(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream(), mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(), mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams(), mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile(), mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::setRefDistance(), and mrpt::nav::CParameterizedTrajectoryGenerator::setRefDistance().
|
staticprotected |
Definition at line 42 of file CPTG_DiffDrive_C.h.
|
protectedinherited |
Definition at line 143 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::simulateTrajectories().
|
protectedinherited |
Definition at line 142 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getMax_V(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getMaxLinVel(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadDefaultParams(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveToConfigFile().
|
protectedinherited |
Definition at line 142 of file CPTG_DiffDrive_CollisionGridBased.h.
Referenced by mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getMax_W(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::getMaxAngVel(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_initialize(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadDefaultParams(), mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile(), and mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveToConfigFile().
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020 |