MRPT  2.0.4
CPTG_Holo_Blend.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
13 
14 namespace mrpt::nav
15 {
16 /** A PTG for circular-shaped robots with holonomic kinematics.
17  * - **Compatible kinematics**: Holonomic robot capable of velocity commands
18  * with a linear interpolation ("ramp "or "blending") time. See
19  * mrpt::kinematics::CVehicleSimul_Holo
20  * - **Compatible robot shape**: Circular robots
21  * - **PTG parameters**: Use the app `ptg-configurator`
22  *
23  * \note [New in MRPT 1.5.0]
24  * \ingroup nav_tpspace
25  */
27 {
29  public:
32  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection);
33  ~CPTG_Holo_Blend() override;
34 
35  void loadFromConfigFile(
37  const std::string& sSection) override;
38  void saveToConfigFile(
40  const std::string& sSection) const override;
41  void loadDefaultParams() override;
42  bool supportVelCmdNOP() const override;
43  double maxTimeInVelCmdNOP(int path_k) const override;
44 
45  std::string getDescription() const override;
46  bool inverseMap_WS2TP(
47  double x, double y, int& out_k, double& out_d,
48  double tolerance_dist = 0.10) const override;
49  bool PTG_IsIntoDomain(double x, double y) const override;
50  void onNewNavDynamicState() override;
51 
52  /** Converts a discretized "alpha" value into a feasible motion command or
53  * action. See derived classes for the meaning of these actions */
55  uint16_t k) const override;
57  const override;
58 
59  size_t getPathStepCount(uint16_t k) const override;
60  void getPathPose(
61  uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const override;
62  double getPathDist(uint16_t k, uint32_t step) const override;
63  bool getPathStepForDist(
64  uint16_t k, double dist, uint32_t& out_step) const override;
65  double getPathStepDuration() const override;
66  double getMaxLinVel() const override { return V_MAX; }
67  double getMaxAngVel() const override { return W_MAX; }
68  void updateTPObstacle(
69  double ox, double oy, std::vector<double>& tp_obstacles) const override;
71  double ox, double oy, uint16_t k, double& tp_obstacle_k) const override;
72 
73  /** Duration of each PTG "step" (default: 10e-3=10 ms) */
74  static double PATH_TIME_STEP;
75  /** Mathematical "epsilon", to detect ill-conditioned situations (e.g. 1/0)
76  * (Default: 1e-4) */
77  static double eps;
78 
79  protected:
80  double T_ramp_max{-1.0};
81  double V_MAX{-1.0}, W_MAX{-1.0};
82  double turningRadiusReference{0.30};
83 
84  std::string expr_V, expr_W, expr_T_ramp;
85  mutable std::vector<int> m_pathStepCountCache;
86 
87  // Compilation of user-given expressions
89  double m_expr_dir; // Used as symbol "dir" in m_expr_v and m_expr_w
90 
91  /** Evals expr_v */
92  double internal_get_v(const double dir) const;
93  /** Evals expr_w */
94  double internal_get_w(const double dir) const;
95  /** Evals expr_T_ramp */
96  double internal_get_T_ramp(const double dir) const;
97 
99 
100  void internal_processNewRobotShape() override;
101  void internal_initialize(
102  const std::string& cacheFilename = std::string(),
103  const bool verbose = true) override;
104  void internal_deinitialize() override;
105 
106  public:
107  /** Axiliary function for computing the line-integral distance along the
108  * trajectory, handling special cases of 1/0: */
109  static double calc_trans_distance_t_below_Tramp(
110  double k2, double k4, double vxi, double vyi, double t);
111  /** Axiliary function for calc_trans_distance_t_below_Tramp() and others */
113  double t, double a, double b, double c);
114 };
115 } // namespace mrpt::nav
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-Spac...
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
Base class for all PTGs using a 2D circular robot shape model.
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.
A wrapper of exprtk runtime expression compiler: it takes a string representing an expression (from a...
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 < ...
double getMaxAngVel() const override
Returns the maximum angular velocity expected from this PTG [rad/s].
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
void internal_deinitialize() override
This must be called to de-initialize the PTG if some parameter is to be changed.
static double calc_trans_distance_t_below_Tramp(double k2, double k4, double vxi, double vyi, double t)
Axiliary function for computing the line-integral distance along the trajectory, handling special cas...
double maxTimeInVelCmdNOP(int path_k) const override
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
double internal_get_v(const double dir) const
Evals expr_v.
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.
This class allows loading and storing values and vectors of different types from a configuration text...
A PTG for circular-shaped robots with holonomic kinematics.
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".
mrpt::expr::CRuntimeCompiledExpression m_expr_v
std::vector< int > m_pathStepCountCache
double internal_get_w(const double dir) const
Evals expr_w.
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
void onNewNavDynamicState() override
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
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)
bool supportVelCmdNOP() const override
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).
bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const override
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
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.
static double calc_trans_distance_t_below_Tramp_abc(double t, double a, double b, double c)
Axiliary function for calc_trans_distance_t_below_Tramp() and others.
static double PATH_TIME_STEP
Duration of each PTG "step" (default: 10e-3=10 ms)
double internal_get_T_ramp(const double dir) const
Evals expr_T_ramp.
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::expr::CRuntimeCompiledExpression m_expr_T_ramp
void internal_processNewRobotShape() override
Will be called whenever the robot shape is set / updated.
double getMaxLinVel() const override
Returns the maximum linear velocity expected from this PTG [m/s].
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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.
static double eps
Mathematical "epsilon", to detect ill-conditioned situations (e.g.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
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. ...
mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const override
Converts a discretized "alpha" value into a feasible motion command or action.
mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
mrpt::expr::CRuntimeCompiledExpression m_expr_w



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020