Main MRPT website > C++ reference for MRPT 1.5.9
CParameterizedTrajectoryGenerator.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
12 #include <mrpt/utils/CStream.h>
14 #include <mrpt/system/filesystem.h>
15 #include <mrpt/system/os.h>
17 
18 using namespace mrpt::nav;
19 
22 
23 
25 
27  refDistance(.0),
28  m_alphaValuesCount(0),
29  m_score_priority(1.0),
30  m_clearance_num_points(5),
31  m_clearance_decimated_paths(15),
32  m_nav_dyn_state(),
33  m_nav_dyn_state_target_k(INVALID_PTG_PATH_INDEX),
34  m_is_initialized(false)
35 { }
36 
38 {
39  m_alphaValuesCount = 121;
40  refDistance = 6.0;
41  m_score_priority = 1.0;
44 }
45 
47 {
48  return false;
49 }
51 {
52  return .0;
53 }
54 
55 
57 {
59  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT (refDistance , double, cfg,sSection);
60  MRPT_LOAD_HERE_CONFIG_VAR(score_priority , double, m_score_priority, cfg,sSection);
61  MRPT_LOAD_HERE_CONFIG_VAR(clearance_num_points, double, m_clearance_num_points, cfg, sSection);
62  MRPT_LOAD_HERE_CONFIG_VAR(clearance_decimated_paths, double, m_clearance_decimated_paths, cfg, sSection);
63 
64  // Ensure a minimum of resolution:
66 
67  // Optional params, for debugging only
68  MRPT_LOAD_HERE_CONFIG_VAR(vxi, double, m_nav_dyn_state.curVelLocal.vx, cfg, sSection);
69  MRPT_LOAD_HERE_CONFIG_VAR(vyi, double, m_nav_dyn_state.curVelLocal.vy, cfg, sSection);
71 
72  MRPT_LOAD_HERE_CONFIG_VAR(reltrg_x, double, m_nav_dyn_state.relTarget.x, cfg, sSection);
73  MRPT_LOAD_HERE_CONFIG_VAR(reltrg_y, double, m_nav_dyn_state.relTarget.y, cfg, sSection);
74  MRPT_LOAD_HERE_CONFIG_VAR_DEGREES(reltrg_phi, double, m_nav_dyn_state.relTarget.phi , cfg, sSection);
75 
76  MRPT_LOAD_HERE_CONFIG_VAR(target_rel_speed, double, m_nav_dyn_state.targetRelSpeed, cfg, sSection);
77 }
79 {
81  const int WN = 25, WV = 30;
82 
83  cfg.write(sSection,"num_paths",m_alphaValuesCount, WN,WV, "Number of discrete paths (`resolution`) in the PTG");
84  cfg.write(sSection,"refDistance",refDistance, WN,WV, "Maximum distance (meters) for building trajectories (visibility range)");
85  cfg.write(sSection,"score_priority",m_score_priority, WN,WV, "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.");
86  cfg.write(sSection, "clearance_num_points", m_clearance_num_points, WN, WV, "Number of steps for the piecewise-constant approximation of clearance (Default=5).");
87  cfg.write(sSection, "clearance_decimated_paths", m_clearance_decimated_paths, WN, WV, "Number of decimated paths for estimation of clearance (Default=15).");
88 
89  // Optional params, for debugging only
90  cfg.write(sSection, "vxi", m_nav_dyn_state.curVelLocal.vx, WN, WV, "(Only for debugging) Current robot velocity vx [m/s].");
91  cfg.write(sSection, "vyi", m_nav_dyn_state.curVelLocal.vy, WN, WV, "(Only for debugging) Current robot velocity vy [m/s].");
92  cfg.write(sSection, "wi", mrpt::utils::RAD2DEG(m_nav_dyn_state.curVelLocal.omega), WN, WV, "(Only for debugging) Current robot velocity omega [deg/s].");
93 
94  cfg.write(sSection, "reltrg_x", m_nav_dyn_state.relTarget.x, WN, WV, "(Only for debugging) Relative target x [m].");
95  cfg.write(sSection, "reltrg_y", m_nav_dyn_state.relTarget.y, WN, WV, "(Only for debugging) Relative target y [m].");
96  cfg.write(sSection, "reltrg_phi", mrpt::utils::RAD2DEG(m_nav_dyn_state.relTarget.phi), WN, WV, "(Only for debugging) Relative target phi [deg].");
97 
98  cfg.write(sSection, "target_rel_speed", m_nav_dyn_state.targetRelSpeed, WN, WV, "(Only for debugging) Desired relative speed at target [0,1]");
99 
100  MRPT_END
101 }
102 
104 {
105  this->deinitialize();
106 
108  in >> version;
109  switch (version)
110  {
111  case 0:
112  case 1:
113  case 2:
114  case 3:
115  case 4:
117  if (version >= 1) in >> m_clearance_num_points;
118  if (version == 2) {
119  bool old_use_approx_clearance;
120  in >> old_use_approx_clearance; // ignored in v>=3
121  }
122  if (version >= 4) {
124  }
125  else {
127  }
128  break;
129  default:
131  };
132 }
133 
135 {
136  const uint8_t version = 4;
137  out << version;
138 
140  out << m_clearance_decimated_paths /* v4*/;
141 }
142 
143 double CParameterizedTrajectoryGenerator::index2alpha(uint16_t k, const unsigned int num_paths)
144 {
145  ASSERT_BELOW_(k, num_paths)
146 
147  return M_PI * (-1.0 + 2.0 * (k + 0.5) / num_paths);
148 }
149 
151 {
152  return index2alpha(k, m_alphaValuesCount);
153 }
154 
155 uint16_t CParameterizedTrajectoryGenerator::alpha2index(double alpha, const unsigned int num_paths)
156 {
158  int k = mrpt::utils::round(0.5*(num_paths*(1.0 + alpha / M_PI) - 1.0));
159  if (k<0) k = 0;
160  if (k >= static_cast<int>(num_paths) ) k = num_paths - 1;
161  return (uint16_t)k;
162 }
163 
165 {
167 }
168 
170  const uint16_t k,
172  const double decimate_distance,
173  const double max_path_distance) const
174 {
175  const size_t nPointsInPath = getPathStepCount(k);
176 
177  bool first=true;
178  // Decimate trajectories: we don't need centimeter resolution!
179  double last_added_dist = 0.0;
180  for (size_t n=0;n<nPointsInPath;n++)
181  {
182  const double d = this->getPathDist(k, n); // distance thru path "k" until timestep "n"
183 
184  // Draw the TP only until we reach the target of the "motion" segment:
185  if (max_path_distance>=0.0 && d>=max_path_distance) break;
186 
187  if (d<last_added_dist+decimate_distance && n!=0)
188  continue; // skip: decimation
189 
190  last_added_dist = d;
191 
193  this->getPathPose(k, n, p);
194 
195  if (first) {
196  first=false;
197  gl_obj.appendLine(0,0,0, p.x, p.y,0);
198  }
199  else gl_obj.appendLineStrip(p.x, p.y,0);
200  }
201 }
202 
203 
204 void CParameterizedTrajectoryGenerator::initTPObstacles(std::vector<double> &TP_Obstacles) const
205 {
206  TP_Obstacles.resize(m_alphaValuesCount);
207  for (size_t k = 0; k < m_alphaValuesCount; k++)
208  initTPObstacleSingle(k, TP_Obstacles[k]);
209 }
211 {
212  TP_Obstacle_k = std::min(
213  refDistance,
216  :
217  this->getPathDist(k, this->getPathStepCount(k) - 1)
218  );
219 }
220 
221 
223 {
224  using namespace mrpt::system;
225  using namespace std;
226 
228 
230  mrpt::system::createDirectory( mrpt::format("%s/PTGs",sPath) );
231 
232  const string sFilTxt_x = mrpt::format("%s/PTGs/PTG%s_x.txt",sPath,ptg_name.c_str() );
233  const string sFilTxt_y = mrpt::format("%s/PTGs/PTG%s_y.txt",sPath,ptg_name.c_str() );
234  const string sFilTxt_phi = mrpt::format("%s/PTGs/PTG%s_phi.txt",sPath,ptg_name.c_str() );
235  const string sFilTxt_t = mrpt::format("%s/PTGs/PTG%s_t.txt",sPath,ptg_name.c_str() );
236  const string sFilTxt_d = mrpt::format("%s/PTGs/PTG%s_d.txt",sPath,ptg_name.c_str() );
237 
238  ofstream fx(sFilTxt_x.c_str()); if (!fx.is_open()) return false;
239  ofstream fy(sFilTxt_y.c_str()); if (!fy.is_open()) return false;
240  ofstream fp(sFilTxt_phi.c_str());if (!fp.is_open()) return false;
241  ofstream fd(sFilTxt_d.c_str()); if (!fd.is_open()) return false;
242 
243  const size_t nPaths = getAlphaValuesCount();
244 
245  // Text version:
246  fx << "% PTG data file for 'x'. Each row is the trajectory for a different 'alpha' parameter value." << endl;
247  fy << "% PTG data file for 'y'. Each row is the trajectory for a different 'alpha' parameter value." << endl;
248  fp << "% PTG data file for 'phi'. Each row is the trajectory for a different 'alpha' parameter value." << endl;
249  fd << "% PTG data file for 'd'. Each row is the trajectory for a different 'alpha' parameter value." << endl;
250 
251  vector<size_t> path_length(nPaths);
252  for (size_t k=0;k<nPaths;k++)
253  path_length[k] = getPathStepCount(k);
254 
255  size_t maxPoints=0;
256  for (size_t k=0;k<nPaths;k++)
257  maxPoints = max( maxPoints, path_length[k] );
258 
259  for (size_t k=0;k<nPaths;k++)
260  {
261  for (size_t n=0;n< maxPoints;n++)
262  {
263  const size_t nn = std::min( n, path_length[k]-1 );
265  this->getPathPose(k,nn, p);
266  fx << p.x << " ";
267  fy << p.y << " ";
268  fp << p.phi << " ";
269  fd << this->getPathDist(k,nn) << " ";
270  }
271  fx << endl;
272  fy << endl;
273  fp << endl;
274  fd << endl;
275  }
276 
277  return true;
278 }
279 
280 
282 {
283  return m_is_initialized;
284 }
285 
287 {
288  // Make sure there is a real difference: notifying a PTG that a condition changed
289  // may imply a significant computational cost if paths need to be re-evaluated on the fly, etc.
290  // so the cost of the comparison here is totally worth:
291  if (force_update || m_nav_dyn_state!=newState)
292  {
293  ASSERT_(newState.targetRelSpeed >= .0 && newState.targetRelSpeed <= 1.0); // sanity check
294  m_nav_dyn_state = newState;
295 
296  // 1st) Build PTG paths without counting for target slow-down:
298 
299  this->onNewNavDynamicState();
300 
301  // 2nd) Save the special path for slow-down:
302  if (this->supportSpeedAtTarget())
303  {
304  int target_k=-1;
305  double target_norm_d;
306  //bool is_exact = // JLB removed this constraint for being too restrictive.
307  this->inverseMap_WS2TP(m_nav_dyn_state.relTarget.x, m_nav_dyn_state.relTarget.y, target_k, target_norm_d,1.0 /*large tolerance*/);
308  if (target_norm_d>0.01 && target_norm_d<0.99 && target_k>=0 && target_k<m_alphaValuesCount)
309  {
310  m_nav_dyn_state_target_k = target_k;
311  this->onNewNavDynamicState(); // Recalc
312  }
313  }
314  }
315 }
316 
317 void CParameterizedTrajectoryGenerator::initialize(const std::string & cacheFilename, const bool verbose)
318 {
319  if (m_is_initialized) return;
320 
321  const std::string sCache = !cacheFilename.empty() ?
322  cacheFilename
323  :
325 
326  this->internal_initialize(sCache,verbose);
327  m_is_initialized = true;
328 }
330 {
331  if (!m_is_initialized) return;
332  this->internal_deinitialize();
333  m_is_initialized = false;
334 }
335 
336 void CParameterizedTrajectoryGenerator::internal_TPObsDistancePostprocess(const double ox, const double oy, const double new_tp_obs_dist, double &inout_tp_obs) const
337 {
338  const bool is_obs_inside_robot_shape = isPointInsideRobotShape(ox,oy);
339  if (!is_obs_inside_robot_shape)
340  {
341  mrpt::utils::keep_min(inout_tp_obs, new_tp_obs_dist);
342  return;
343  }
344 
345  // Handle the special case of obstacles *inside* the robot at the begining of the PTG path:
346  switch (COLLISION_BEHAVIOR)
347  {
348  case COLL_BEH_STOP:
349  inout_tp_obs = .0;
350  break;
351 
352  case COLL_BEH_BACK_AWAY:
353  {
354  if (new_tp_obs_dist < getMaxRobotRadius() ) {
355  // This means that we are getting apart of the obstacle:
356  // ignore it to allow the robot to get off the near-collision:
357  // Don't change inout_tp_obs.
358  return;
359  }
360  else {
361  // This means we are already in collision and trying to get even closer
362  // to the obstacle: totally disprove this action:
363  inout_tp_obs = .0;
364  }
365  }
366  break;
367 
368  default:
369  THROW_EXCEPTION("Obstacle postprocessing enum not implemented!");
370  }
371 }
372 
374 {
376  for (unsigned int decim_k = 0; decim_k < m_clearance_decimated_paths; decim_k++)
377  {
378  const auto real_k = cd.decimated_k_to_real_k(decim_k);
379  const size_t numPathSteps = getPathStepCount(real_k);
380  const double numStepsPerIncr = (numPathSteps - 1.0) / double(m_clearance_num_points);
381 
382  auto & cl_path = cd.get_path_clearance_decimated(decim_k);
383  for (double step_pointer_dbl = 0.0; step_pointer_dbl < numPathSteps; step_pointer_dbl += numStepsPerIncr)
384  {
385  const size_t step = mrpt::utils::round(step_pointer_dbl);
386  const double dist_over_path = this->getPathDist(real_k, step);
387  cl_path[dist_over_path] = 1.0; // create entry in map<>
388  }
389  }
390 }
391 
392 void CParameterizedTrajectoryGenerator::updateClearance(const double ox, const double oy, ClearanceDiagram & cd) const
393 {
394  // Initialize CD on first call:
397 
398  // evaluate in derived-class: this function also keeps the minimum automatically.
399  for (uint16_t decim_k = 0; decim_k < cd.get_decimated_num_paths(); decim_k++)
400  {
401  const auto real_k = cd.decimated_k_to_real_k(decim_k);
402  this->evalClearanceSingleObstacle(ox, oy, real_k, cd.get_path_clearance_decimated(decim_k));
403  }
404 }
405 
406 void CParameterizedTrajectoryGenerator::updateClearancePost(ClearanceDiagram & cd, const std::vector<double> &TP_obstacles) const
407 {
408  // Used only when in approx mode (Removed 30/01/2017)
409 }
410 
411 void CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle(const double ox, const double oy, const uint16_t k, ClearanceDiagram::dist2clearance_t & inout_realdist2clearance, bool treat_as_obstacle) const
412 {
413  bool had_collision = false;
414 
415  const size_t numPathSteps = getPathStepCount(k);
416  // We don't have steps enought (?). Just ignore clearance for this short
417  // path in this "k" direction:
418  if (numPathSteps <= inout_realdist2clearance.size())
419  {
420  std::cerr << "[CParameterizedTrajectoryGenerator::"
421  "evalClearanceSingleObstacle] Warning: k="
422  << k << " numPathSteps is only=" << numPathSteps
423  << " num of clearance steps="
424  << inout_realdist2clearance.size();
425  return;
426  }
427 
428  const double numStepsPerIncr = (numPathSteps - 1.0) / (inout_realdist2clearance.size());
429 
430  double step_pointer_dbl = 0.0;
431  const mrpt::math::TPoint2D og(ox,oy); // obstacle in "global" frame
432  mrpt::math::TPoint2D ol; // obstacle in robot frame
433 
434  for (auto &e : inout_realdist2clearance)
435  {
436  step_pointer_dbl += numStepsPerIncr;
437  const size_t step = mrpt::utils::round(step_pointer_dbl);
438  const double dist_over_path = e.first;
439  double & inout_clearance = e.second;
440 
441  if (had_collision) {
442  // We found a collision in a previous step along this "k" path, so
443  // it does not make sense to evaluate the clearance of a pose which is not reachable:
444  inout_clearance = .0;
445  continue;
446  }
447 
448  mrpt::math::TPose2D pose;
449  this->getPathPose(k, step, pose);
450 
451  // obstacle to robot clearance:
452  pose.inverseComposePoint(og, ol);
453  const double this_clearance = treat_as_obstacle ?
454  this->evalClearanceToRobotShape(ol.x, ol.y)
455  :
456  ol.norm()
457  ;
458  if (this_clearance <= .0 && treat_as_obstacle &&
459  (dist_over_path>0.5 || std::abs(mrpt::math::angDistance(std::atan2(oy,ox),index2alpha(k)))<mrpt::utils::DEG2RAD(45.0)))
460  {
461  // Collision:
462  had_collision = true;
463  inout_clearance = .0;
464  }
465  else
466  {
467  // The obstacle is not a direct collision.
468  const double this_clearance_norm = this_clearance / this->refDistance;
469 
470  // Update minimum in output structure
471  mrpt::utils::keep_min(inout_clearance, this_clearance_norm);
472  }
473  }
474 }
475 
477  curVelLocal(0,0,0),
478  relTarget(20.0,0,0), // Default: assume a "distant" target ahead
479  targetRelSpeed(0)
480 {
481 }
482 
484 {
485  return
486  (curVelLocal==o.curVelLocal) &&
487  (relTarget==o.relTarget) &&
488  (targetRelSpeed==o.targetRelSpeed)
489  ;
490 }
491 
493 {
494  const uint8_t version = 0;
495  out << version;
496  // Data:
497  out << curVelLocal << relTarget << targetRelSpeed;
498 }
499 
501 {
503  in >> version;
504  switch (version)
505  {
506  case 0:
507  in >>curVelLocal >> relTarget >> targetRelSpeed;
508  break;
509  default:
511  };
512 }
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
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-Spac...
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).
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
double targetRelSpeed
Desired relative speed [0,1] at target. Default=0.
size_t get_decimated_num_paths() const
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
double y
X,Y coordinates.
virtual void internal_initialize(const std::string &cacheFilename=std::string(), const bool verbose=true)=0
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:46
double DEG2RAD(const double x)
Degrees to radians.
void appendLineStrip(float x, float y, float z)
Appends a line whose starting point is the end point of the last line (similar to OpenGL&#39;s GL_LINE_ST...
Definition: CSetOfLines.h:84
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
dist2clearance_t & get_path_clearance_decimated(size_t decim_k)
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)...
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
GLint * first
Definition: glext.h:3703
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(variableName, variableType, configFileObject, sectionNameStr)
#define THROW_EXCEPTION(msg)
#define ASSERT_BELOW_(__A, __B)
GLenum GLsizei n
Definition: glext.h:4618
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) MRPT_OVERRIDE
Parameters accepted by this base class:
Favor getting back from too-close (almost collision) obstacles.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:91
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
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 dynam...
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.
STL namespace.
#define M_PI
Definition: bits.h:78
std::string BASE_IMPEXP fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:315
virtual size_t getPathStepCount(uint16_t k) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
virtual double getPathDist(uint16_t k, uint32_t step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
virtual void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
bool debugDumpInFiles(const std::string &ptg_name) const
Dump PTG trajectories in four text files: `.
virtual void internal_writeToStream(mrpt::utils::CStream &out) const
This class allows loading and storing values and vectors of different types from a configuration text...
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 set...
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
unsigned char uint8_t
Definition: rptypes.h:43
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This is the base class for any user-defined PTG.
static std::string OUTPUT_DEBUG_PATH_PREFIX
The path used as defaul output in, for example, debugDumpInFiles. (Default="./reactivenav.logs/")
size_t decimated_k_to_real_k(size_t k) const
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
Definition: CSetOfLines.h:70
double RAD2DEG(const double x)
Radians to degrees.
virtual void internal_readFromStream(mrpt::utils::CStream &in)
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
#define MRPT_END
size_t get_actual_num_paths() const
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose */
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
bool isInitialized() const
Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queri...
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
virtual void onNewNavDynamicState()=0
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
double vy
Velocity components: X,Y (m/s)
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance() ...
mrpt::math::TPose2D relTarget
Current relative target location.
int version
Definition: mrpt_jpeglib.h:898
void resize(size_t actual_num_paths, size_t decimated_num_paths)
Initializes the container to allocate decimated_num_paths entries, as a decimated subset of a total o...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
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.
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...
double norm() const
Point norm.
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
double y
X,Y coordinates.
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 pa...
#define MRPT_START
unsigned __int64 uint64_t
Definition: rptypes.h:52
void deinitialize()
This must be called to de-initialize the PTG if some parameter is to be changed.
#define MRPT_LOAD_HERE_CONFIG_VAR(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void write(const std::string &section, const std::string &name, const data_t &value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
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 ...
GLuint in
Definition: glext.h:6301
Lightweight 2D pose.
#define ASSERT_(f)
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
virtual double evalClearanceToRobotShape(const double ox, const double oy) const =0
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center...
void inverseComposePoint(const TPoint2D g, TPoint2D &l) const
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:35
Lightweight 2D point.
TNavDynamicState m_nav_dyn_state
Updated before each nav step by.
Dynamic state that may affect the PTG path parameterization.
GLfloat GLfloat p
Definition: glext.h:5587
virtual void internal_deinitialize()=0
This must be called to de-initialize the PTG if some parameter is to be changed.
double phi
Orientation (rads)
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
virtual double getMaxRobotRadius() const =0
Returns an approximation of the robot radius.
double omega
Angular velocity (rad/s)
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_normalized_d, double tolerance_dist=0.10) const =0
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020