MRPT  2.0.3
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
mrpt::nav::CPTG_RobotShape_Circular
Base class for all PTGs using a 2D circular robot shape model.
Definition: CParameterizedTrajectoryGenerator.h:565
CParameterizedTrajectoryGenerator.h
mrpt::nav::CPTG_Holo_Blend::inverseMap_WS2TP
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...
Definition: CPTG_Holo_Blend.cpp:309
mrpt::nav::CPTG_Holo_Blend::maxTimeInVelCmdNOP
double maxTimeInVelCmdNOP(int path_k) const override
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
Definition: CPTG_Holo_Blend.cpp:777
mrpt::nav::CPTG_Holo_Blend::T_ramp_max
double T_ramp_max
Definition: CPTG_Holo_Blend.h:80
mrpt::nav::CPTG_Holo_Blend::supportVelCmdNOP
bool supportVelCmdNOP() const override
Returns true if it is possible to stop sending velocity commands to the robot and,...
Definition: CPTG_Holo_Blend.cpp:776
mrpt::nav::CPTG_Holo_Blend::getPathStepForDist
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 < ...
Definition: CPTG_Holo_Blend.cpp:535
mrpt::nav::CPTG_Holo_Blend::m_expr_v
mrpt::expr::CRuntimeCompiledExpression m_expr_v
Definition: CPTG_Holo_Blend.h:88
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
verbose
params verbose
Definition: chessboard_stereo_camera_calib_unittest.cpp:59
mrpt::nav::CPTG_Holo_Blend::m_pathStepCountCache
std::vector< int > m_pathStepCountCache
Definition: CPTG_Holo_Blend.h:85
mrpt::nav::CPTG_Holo_Blend::eps
static double eps
Mathematical "epsilon", to detect ill-conditioned situations (e.g.
Definition: CPTG_Holo_Blend.h:77
mrpt::nav::CPTG_Holo_Blend::turningRadiusReference
double turningRadiusReference
Definition: CPTG_Holo_Blend.h:82
mrpt::nav::CPTG_Holo_Blend::W_MAX
double W_MAX
Definition: CPTG_Holo_Blend.h:81
mrpt::nav::CPTG_Holo_Blend::getPathPose
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.
Definition: CPTG_Holo_Blend.cpp:455
mrpt::nav::CPTG_Holo_Blend::saveToConfigFile
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.
Definition: CPTG_Holo_Blend.cpp:218
mrpt::nav::CPTG_Holo_Blend::m_expr_w
mrpt::expr::CRuntimeCompiledExpression m_expr_w
Definition: CPTG_Holo_Blend.h:88
mrpt::nav::CPTG_Holo_Blend::updateTPObstacleSingle
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.
Definition: CPTG_Holo_Blend.cpp:630
mrpt::nav::CPTG_Holo_Blend::CPTG_Holo_Blend
CPTG_Holo_Blend()
Definition: CPTG_Holo_Blend.cpp:791
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::nav::CPTG_Holo_Blend::getPathStepDuration
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".
Definition: CPTG_Holo_Blend.cpp:790
mrpt::nav::CPTG_Holo_Blend::V_MAX
double V_MAX
Definition: CPTG_Holo_Blend.h:81
mrpt::nav::CPTG_Holo_Blend::getPathStepCount
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.
Definition: CPTG_Holo_Blend.cpp:434
mrpt::nav::CPTG_Holo_Blend::loadDefaultParams
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Definition: CPTG_Holo_Blend.cpp:189
mrpt::nav::CPTG_Holo_Blend::m_expr_dir
double m_expr_dir
Definition: CPTG_Holo_Blend.h:89
CRuntimeCompiledExpression.h
mrpt::nav::CPTG_Holo_Blend::getSupportedKinematicVelocityCommand
mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
Definition: CPTG_Holo_Blend.cpp:770
mrpt::nav::CPTG_Holo_Blend::PTG_IsIntoDomain
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
Definition: CPTG_Holo_Blend.cpp:408
mrpt::nav::CPTG_Holo_Blend::expr_V
std::string expr_V
Definition: CPTG_Holo_Blend.h:84
mrpt::nav::CPTG_Holo_Blend::getMaxLinVel
double getMaxLinVel() const override
Returns the maximum linear velocity expected from this PTG [m/s].
Definition: CPTG_Holo_Blend.h:66
mrpt::nav::CPTG_Holo_Blend::updateTPObstacle
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,...
Definition: CPTG_Holo_Blend.cpp:753
mrpt::nav::CPTG_Holo_Blend::internal_construct_exprs
void internal_construct_exprs()
Definition: CPTG_Holo_Blend.cpp:801
mrpt::nav::CPTG_Holo_Blend::internal_get_T_ramp
double internal_get_T_ramp(const double dir) const
Evals expr_T_ramp.
Definition: CPTG_Holo_Blend.cpp:829
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::nav::CPTG_Holo_Blend::getDescription
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
Definition: CPTG_Holo_Blend.cpp:259
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::nav::CPTG_Holo_Blend::internal_processNewRobotShape
void internal_processNewRobotShape() override
Will be called whenever the robot shape is set / updated.
Definition: CPTG_Holo_Blend.cpp:764
mrpt::expr::CRuntimeCompiledExpression
A wrapper of exprtk runtime expression compiler: it takes a string representing an expression (from a...
Definition: CRuntimeCompiledExpression.h:61
mrpt::nav::CPTG_Holo_Blend::PATH_TIME_STEP
static double PATH_TIME_STEP
Duration of each PTG "step" (default: 10e-3=10 ms)
Definition: CPTG_Holo_Blend.h:74
mrpt::nav::CPTG_Holo_Blend::onNewNavDynamicState
void onNewNavDynamicState() override
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
Definition: CPTG_Holo_Blend.cpp:184
mrpt::nav::CPTG_Holo_Blend::internal_get_v
double internal_get_v(const double dir) const
Evals expr_v.
Definition: CPTG_Holo_Blend.cpp:819
mrpt::nav::CPTG_Holo_Blend::internal_deinitialize
void internal_deinitialize() override
This must be called to de-initialize the PTG if some parameter is to be changed.
Definition: CPTG_Holo_Blend.cpp:415
mrpt::kinematics::CVehicleVelCmd::Ptr
std::shared_ptr< CVehicleVelCmd > Ptr
Definition: CVehicleVelCmd.h:22
mrpt::nav::CPTG_Holo_Blend::getMaxAngVel
double getMaxAngVel() const override
Returns the maximum angular velocity expected from this PTG [rad/s].
Definition: CPTG_Holo_Blend.h:67
mrpt::nav::CPTG_Holo_Blend::expr_W
std::string expr_W
Definition: CPTG_Holo_Blend.h:84
mrpt::nav::CPTG_Holo_Blend::~CPTG_Holo_Blend
~CPTG_Holo_Blend() override
mrpt::nav::CPTG_Holo_Blend
A PTG for circular-shaped robots with holonomic kinematics.
Definition: CPTG_Holo_Blend.h:26
mrpt::nav::CPTG_Holo_Blend::directionToMotionCommand
mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const override
Converts a discretized "alpha" value into a feasible motion command or action.
Definition: CPTG_Holo_Blend.cpp:420
dir
const auto dir
Definition: chessboard_stereo_camera_calib_unittest.cpp:28
mrpt::nav::CPTG_Holo_Blend::internal_get_w
double internal_get_w(const double dir) const
Evals expr_w.
Definition: CPTG_Holo_Blend.cpp:824
mrpt::nav::CPTG_Holo_Blend::calc_trans_distance_t_below_Tramp
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...
Definition: CPTG_Holo_Blend.cpp:152
mrpt::nav::CPTG_Holo_Blend::getPathDist
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.
Definition: CPTG_Holo_Blend.cpp:511
mrpt::nav::CPTG_Holo_Blend::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
Definition: CPTG_Holo_Blend.cpp:200
mrpt::nav::CPTG_Holo_Blend::m_expr_T_ramp
mrpt::expr::CRuntimeCompiledExpression m_expr_T_ramp
Definition: CPTG_Holo_Blend.h:88
mrpt::nav::CPTG_Holo_Blend::expr_T_ramp
std::string expr_T_ramp
Definition: CPTG_Holo_Blend.h:84
mrpt::nav::CPTG_Holo_Blend::internal_initialize
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...
Definition: CPTG_Holo_Blend.cpp:835
mrpt::nav::CPTG_Holo_Blend::calc_trans_distance_t_below_Tramp_abc
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.
Definition: CPTG_Holo_Blend.cpp:136



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020