24 namespace mrpt {
namespace opengl {
class CSetOfLines; } }
62 public
mrpt::utils::CSerializable,
63 public
mrpt::utils::CLoadableOptions
85 virtual std::string getDescription()
const = 0 ;
89 virtual void internal_initialize(
const std::string & cacheFilename = std::string(),
const bool verbose =
true) = 0;
91 virtual void internal_deinitialize() = 0;
103 virtual bool inverseMap_WS2TP(
double x,
double y,
int &out_k,
double &out_normalized_d,
double tolerance_dist = 0.10)
const = 0;
109 return inverseMap_WS2TP(x,y,k,d);
113 virtual bool isBijectiveAt(uint16_t k, uint32_t step)
const {
return true; }
116 virtual mrpt::kinematics::CVehicleVelCmdPtr directionToMotionCommand( uint16_t k )
const = 0;
120 virtual mrpt::kinematics::CVehicleVelCmdPtr getSupportedKinematicVelocityCommand()
const = 0;
138 virtual void onNewNavDynamicState() = 0;
146 virtual size_t getPathStepCount(uint16_t k)
const = 0;
155 virtual double getPathDist(uint16_t k, uint32_t step)
const = 0;
159 virtual double getPathStepDuration()
const = 0;
162 virtual double getMaxLinVel()
const = 0;
164 virtual double getMaxAngVel()
const = 0;
171 virtual bool getPathStepForDist(uint16_t k,
double dist, uint32_t &out_step)
const = 0;
181 virtual void updateTPObstacle(
double ox,
double oy, std::vector<double> &tp_obstacles)
const = 0;
184 virtual void updateTPObstacleSingle(
double ox,
double oy, uint16_t k,
double &tp_obstacle_k)
const = 0;
188 virtual void loadDefaultParams();
193 virtual bool supportVelCmdNOP()
const;
204 virtual double maxTimeInVelCmdNOP(
int path_k)
const;
214 virtual double getMaxRobotRadius()
const = 0;
216 virtual bool isPointInsideRobotShape(
const double x,
const double y)
const = 0;
219 virtual double evalClearanceToRobotShape(
const double ox,
const double oy)
const = 0;
224 void updateNavDynamicState(
const TNavDynamicState &newState,
const bool force_update =
false);
230 void initialize(
const std::string & cacheFilename = std::string(),
const bool verbose =
true);
234 bool isInitialized()
const;
242 double index2alpha(uint16_t k)
const;
243 static double index2alpha(uint16_t k,
const unsigned int num_paths);
246 uint16_t alpha2index(
double alpha )
const;
247 static uint16_t alpha2index(
double alpha,
const unsigned int num_paths);
252 void initTPObstacles(std::vector<double> &TP_Obstacles)
const;
253 void initTPObstacleSingle(uint16_t k,
double &TP_Obstacle_k)
const;
271 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;
279 bool debugDumpInFiles(
const std::string &ptg_name)
const;
305 void updateClearance(
const double ox,
const double oy,
ClearanceDiagram & cd)
const;
306 void updateClearancePost(
ClearanceDiagram & cd,
const std::vector<double> &TP_obstacles)
const;
317 static const uint16_t INVALID_PTG_PATH_INDEX =
static_cast<uint16_t
>(-1);
326 void internal_TPObsDistancePostprocess(
const double ox,
const double oy,
const double new_tp_obs_dist,
double &inout_tp_obs)
const;
337 virtual void evalClearanceSingleObstacle(
const double ox,
const double oy,
const uint16_t k,
ClearanceDiagram::dist2clearance_t & inout_realdist2clearance,
bool treat_as_obstacle =
true)
const;
362 virtual double evalClearanceToRobotShape(
const double ox,
const double oy)
const MRPT_OVERRIDE;
364 bool isPointInsideRobotShape(
const double x,
const double y)
const MRPT_OVERRIDE;
367 virtual void internal_processNewRobotShape() = 0;
375 void loadDefaultParams() MRPT_OVERRIDE;
390 void setRobotShapeRadius(
const double robot_radius);
393 virtual double evalClearanceToRobotShape(
const double ox,
const double oy)
const MRPT_OVERRIDE;
396 bool isPointInsideRobotShape(
const double x,
const double y)
const MRPT_OVERRIDE;
398 virtual void internal_processNewRobotShape() = 0;
405 void loadDefaultParams() MRPT_OVERRIDE;
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
unsigned getClearanceDecimatedPaths() const
bool operator==(const TPoint2D &p1, const TPoint2D &p2)
Exact comparison between 2D points.
double targetRelSpeed
Desired relative speed [0,1] at target. Default=0.
double getRefDistance() const
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Base class for all PTGs using a 2D circular robot shape model.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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.
bool operator!=(const TPoint2D &p1, const TPoint2D &p2)
Exact comparison between 2D points.
Favor getting back from too-close (almost collision) obstacles.
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...
A wrapper of a TPolygon2D class, implementing CSerializable.
uint16_t m_nav_dyn_state_target_k
Update in updateNavDynamicState(), contains the path index (k) for the target.
Clearance information for one particular PTG and one set of obstacles.
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
static std::string OUTPUT_DEBUG_PATH_PREFIX
The path used as defaul output in, for example, debugDumpInFiles. (Default="./reactivenav.logs/")
This class allows loading and storing values and vectors of different types from a configuration text...
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
const mrpt::math::CPolygon & getRobotShape() const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
This is the base class for any user-defined PTG.
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
double getRobotShapeRadius() const
const TNavDynamicState & getCurrentNavDynamicState() const
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose */
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...
unsigned getClearanceStepCount() const
Base class for all PTGs using a 2D polygonal robot shape model.
void setScorePriorty(double prior)
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
mrpt::math::TPose2D relTarget
Current relative target location.
void setClearanceStepCount(const unsigned res)
uint16_t m_clearance_num_points
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0...
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
virtual double getActualUnloopedPathLength(uint16_t k) const
Returns the actual distance (in meters) of the path, discounting possible circular loops of the path ...
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG...
virtual void setRefDistance(const double refDist)
void setClearanceDecimatedPaths(const unsigned num)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
std::vector< mrpt::nav::CParameterizedTrajectoryGeneratorPtr > TListPTGPtr
A list of PTGs (smart pointers)
A set of independent lines (or segments), one line with its own start and end positions (X...
virtual ~CParameterizedTrajectoryGenerator()
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
TNavDynamicState m_nav_dyn_state
Updated before each nav step by.
Dynamic state that may affect the PTG path parameterization.
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator * > TListPTGs
A list of PTGs (bare pointers)
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
mrpt::math::CPolygon m_robotShape
virtual bool PTG_IsIntoDomain(double x, double y) const
Returns the same than inverseMap_WS2TP() but without any additional cost.