Main MRPT website > C++ reference for MRPT 1.9.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-2018, 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 
14 #include <mrpt/system/filesystem.h>
15 #include <mrpt/system/os.h>
17 
18 using namespace mrpt::nav;
19 
21  "./reactivenav.logs";
24 
27 
29  : refDistance(.0),
30  m_alphaValuesCount(0),
31  m_score_priority(1.0),
32  m_clearance_num_points(5),
33  m_clearance_decimated_paths(15),
34  m_nav_dyn_state(),
35  m_nav_dyn_state_target_k(INVALID_PTG_PATH_INDEX),
36  m_is_initialized(false)
37 {
38 }
39 
40 void CParameterizedTrajectoryGenerator::loadDefaultParams()
41 {
42  m_alphaValuesCount = 121;
43  refDistance = 6.0;
44  m_score_priority = 1.0;
47 }
48 
50 {
51  return false;
52 }
54 {
55  return .0;
56 }
57 
59  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
60 {
62  num_paths, uint64_t, m_alphaValuesCount, cfg, sSection);
63  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(refDistance, double, cfg, sSection);
65  score_priority, double, m_score_priority, cfg, sSection);
67  clearance_num_points, double, m_clearance_num_points, cfg, sSection);
69  clearance_decimated_paths, double, m_clearance_decimated_paths, cfg,
70  sSection);
71 
72  // Ensure a minimum of resolution:
74 
75  // Optional params, for debugging only
77  vxi, double, m_nav_dyn_state.curVelLocal.vx, cfg, sSection);
79  vyi, double, m_nav_dyn_state.curVelLocal.vy, cfg, sSection);
81  wi, double, m_nav_dyn_state.curVelLocal.omega, cfg, sSection);
82 
84  reltrg_x, double, m_nav_dyn_state.relTarget.x, cfg, sSection);
86  reltrg_y, double, m_nav_dyn_state.relTarget.y, cfg, sSection);
88  reltrg_phi, double, m_nav_dyn_state.relTarget.phi, cfg, sSection);
89 
91  target_rel_speed, double, m_nav_dyn_state.targetRelSpeed, cfg,
92  sSection);
93 }
95  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
96 {
98  const int WN = 25, WV = 30;
99 
100  cfg.write(
101  sSection, "num_paths", m_alphaValuesCount, WN, WV,
102  "Number of discrete paths (`resolution`) in the PTG");
103  cfg.write(
104  sSection, "refDistance", refDistance, WN, WV,
105  "Maximum distance (meters) for building trajectories (visibility "
106  "range)");
107  cfg.write(
108  sSection, "score_priority", m_score_priority, WN, WV,
109  "When used in path planning, a multiplying factor (default=1.0) for "
110  "the scores for this PTG. Assign values <1 to PTGs with low priority.");
111  cfg.write(
112  sSection, "clearance_num_points", m_clearance_num_points, WN, WV,
113  "Number of steps for the piecewise-constant approximation of clearance "
114  "(Default=5).");
115  cfg.write(
116  sSection, "clearance_decimated_paths", m_clearance_decimated_paths, WN,
117  WV,
118  "Number of decimated paths for estimation of clearance (Default=15).");
119 
120  // Optional params, for debugging only
121  cfg.write(
122  sSection, "vxi", m_nav_dyn_state.curVelLocal.vx, WN, WV,
123  "(Only for debugging) Current robot velocity vx [m/s].");
124  cfg.write(
125  sSection, "vyi", m_nav_dyn_state.curVelLocal.vy, WN, WV,
126  "(Only for debugging) Current robot velocity vy [m/s].");
127  cfg.write(
128  sSection, "wi", mrpt::RAD2DEG(m_nav_dyn_state.curVelLocal.omega), WN,
129  WV, "(Only for debugging) Current robot velocity omega [deg/s].");
130 
131  cfg.write(
132  sSection, "reltrg_x", m_nav_dyn_state.relTarget.x, WN, WV,
133  "(Only for debugging) Relative target x [m].");
134  cfg.write(
135  sSection, "reltrg_y", m_nav_dyn_state.relTarget.y, WN, WV,
136  "(Only for debugging) Relative target y [m].");
137  cfg.write(
138  sSection, "reltrg_phi", mrpt::RAD2DEG(m_nav_dyn_state.relTarget.phi),
139  WN, WV, "(Only for debugging) Relative target phi [deg].");
140 
141  cfg.write(
142  sSection, "target_rel_speed", m_nav_dyn_state.targetRelSpeed, WN, WV,
143  "(Only for debugging) Desired relative speed at target [0,1]");
144 
145  MRPT_END
146 }
147 
150 {
151  this->deinitialize();
152 
153  uint8_t version;
154  in >> version;
155  switch (version)
156  {
157  case 0:
158  case 1:
159  case 2:
160  case 3:
161  case 4:
163  if (version >= 1) in >> m_clearance_num_points;
164  if (version == 2)
165  {
166  bool old_use_approx_clearance;
167  in >> old_use_approx_clearance; // ignored in v>=3
168  }
169  if (version >= 4)
170  {
172  }
173  else
174  {
176  }
177  break;
178  default:
180  };
181 }
182 
185 {
186  const uint8_t version = 4;
187  out << version;
188 
190  << m_clearance_num_points /* v1 */;
191  out << m_clearance_decimated_paths /* v4*/;
192 }
193 
195  uint16_t k, const unsigned int num_paths)
196 {
197  ASSERT_BELOW_(k, num_paths);
198  return M_PI * (-1.0 + 2.0 * (k + 0.5) / num_paths);
199 }
200 
202 {
203  return index2alpha(k, m_alphaValuesCount);
204 }
205 
207  double alpha, const unsigned int num_paths)
208 {
210  int k = mrpt::round(0.5 * (num_paths * (1.0 + alpha / M_PI) - 1.0));
211  if (k < 0) k = 0;
212  if (k >= static_cast<int>(num_paths)) k = num_paths - 1;
213  return (uint16_t)k;
214 }
215 
217 {
219 }
220 
222  const uint16_t k, mrpt::opengl::CSetOfLines& gl_obj,
223  const double decimate_distance, const double max_path_distance) const
224 {
225  const size_t nPointsInPath = getPathStepCount(k);
226 
227  bool first = true;
228  // Decimate trajectories: we don't need centimeter resolution!
229  double last_added_dist = 0.0;
230  for (size_t n = 0; n < nPointsInPath; n++)
231  {
232  const double d = this->getPathDist(
233  k, n); // distance thru path "k" until timestep "n"
234 
235  // Draw the TP only until we reach the target of the "motion" segment:
236  if (max_path_distance >= 0.0 && d >= max_path_distance) break;
237 
238  if (d < last_added_dist + decimate_distance && n != 0)
239  continue; // skip: decimation
240 
241  last_added_dist = d;
242 
244  this->getPathPose(k, n, p);
245 
246  if (first)
247  {
248  first = false;
249  gl_obj.appendLine(0, 0, 0, p.x, p.y, 0);
250  }
251  else
252  gl_obj.appendLineStrip(p.x, p.y, 0);
253  }
254 }
255 
257  std::vector<double>& TP_Obstacles) const
258 {
259  TP_Obstacles.resize(m_alphaValuesCount);
260  for (size_t k = 0; k < m_alphaValuesCount; k++)
261  initTPObstacleSingle(k, TP_Obstacles[k]);
262 }
264  uint16_t k, double& TP_Obstacle_k) const
265 {
266  TP_Obstacle_k = std::min(
268  ? refDistance
269  : this->getPathDist(k, this->getPathStepCount(k) - 1));
270 }
271 
273  const std::string& ptg_name) const
274 {
275  using namespace mrpt::system;
276  using namespace std;
277 
278  const char* sPath =
280 
282  mrpt::system::createDirectory(mrpt::format("%s/PTGs", sPath));
283 
284  const string sFilTxt_x =
285  mrpt::format("%s/PTGs/PTG%s_x.txt", sPath, ptg_name.c_str());
286  const string sFilTxt_y =
287  mrpt::format("%s/PTGs/PTG%s_y.txt", sPath, ptg_name.c_str());
288  const string sFilTxt_phi =
289  mrpt::format("%s/PTGs/PTG%s_phi.txt", sPath, ptg_name.c_str());
290  const string sFilTxt_t =
291  mrpt::format("%s/PTGs/PTG%s_t.txt", sPath, ptg_name.c_str());
292  const string sFilTxt_d =
293  mrpt::format("%s/PTGs/PTG%s_d.txt", sPath, ptg_name.c_str());
294 
295  ofstream fx(sFilTxt_x.c_str());
296  if (!fx.is_open()) return false;
297  ofstream fy(sFilTxt_y.c_str());
298  if (!fy.is_open()) return false;
299  ofstream fp(sFilTxt_phi.c_str());
300  if (!fp.is_open()) return false;
301  ofstream fd(sFilTxt_d.c_str());
302  if (!fd.is_open()) return false;
303 
304  const size_t nPaths = getAlphaValuesCount();
305 
306  // Text version:
307  fx << "% PTG data file for 'x'. Each row is the trajectory for a different "
308  "'alpha' parameter value."
309  << endl;
310  fy << "% PTG data file for 'y'. Each row is the trajectory for a different "
311  "'alpha' parameter value."
312  << endl;
313  fp << "% PTG data file for 'phi'. Each row is the trajectory for a "
314  "different 'alpha' parameter value."
315  << endl;
316  fd << "% PTG data file for 'd'. Each row is the trajectory for a different "
317  "'alpha' parameter value."
318  << endl;
319 
320  vector<size_t> path_length(nPaths);
321  for (size_t k = 0; k < nPaths; k++) path_length[k] = getPathStepCount(k);
322 
323  size_t maxPoints = 0;
324  for (size_t k = 0; k < nPaths; k++)
325  maxPoints = max(maxPoints, path_length[k]);
326 
327  for (size_t k = 0; k < nPaths; k++)
328  {
329  for (size_t n = 0; n < maxPoints; n++)
330  {
331  const size_t nn = std::min(n, path_length[k] - 1);
333  this->getPathPose(k, nn, p);
334  fx << p.x << " ";
335  fy << p.y << " ";
336  fp << p.phi << " ";
337  fd << this->getPathDist(k, nn) << " ";
338  }
339  fx << endl;
340  fy << endl;
341  fp << endl;
342  fd << endl;
343  }
344 
345  return true;
346 }
347 
349 {
350  return m_is_initialized;
351 }
352 
355  const bool force_update)
356 {
357  // Make sure there is a real difference: notifying a PTG that a condition
358  // changed
359  // may imply a significant computational cost if paths need to be
360  // re-evaluated on the fly, etc.
361  // so the cost of the comparison here is totally worth:
362  if (force_update || m_nav_dyn_state != newState)
363  {
364  ASSERT_(
365  newState.targetRelSpeed >= .0 &&
366  newState.targetRelSpeed <= 1.0); // sanity check
367  m_nav_dyn_state = newState;
368 
369  // 1st) Build PTG paths without counting for target slow-down:
371 
372  this->onNewNavDynamicState();
373 
374  // 2nd) Save the special path for slow-down:
375  if (this->supportSpeedAtTarget())
376  {
377  int target_k = -1;
378  double target_norm_d;
379  // bool is_exact = // JLB removed this constraint for being too
380  // restrictive.
381  this->inverseMap_WS2TP(
383  target_k, target_norm_d, 1.0 /*large tolerance*/);
384  if (target_norm_d > 0.01 && target_norm_d < 0.99 && target_k >= 0 &&
385  target_k < m_alphaValuesCount)
386  {
387  m_nav_dyn_state_target_k = target_k;
388  this->onNewNavDynamicState(); // Recalc
389  }
390  }
391  }
392 }
393 
395  const std::string& cacheFilename, const bool verbose)
396 {
397  if (m_is_initialized) return;
398 
399  const std::string sCache =
400  !cacheFilename.empty()
401  ? cacheFilename
402  : std::string("cache_") +
404  std::string(".bin.gz");
405 
406  this->internal_initialize(sCache, verbose);
407  m_is_initialized = true;
408 }
410 {
411  if (!m_is_initialized) return;
412  this->internal_deinitialize();
413  m_is_initialized = false;
414 }
415 
417  const double ox, const double oy, const double new_tp_obs_dist,
418  double& inout_tp_obs) const
419 {
420  const bool is_obs_inside_robot_shape = isPointInsideRobotShape(ox, oy);
421  if (!is_obs_inside_robot_shape)
422  {
423  mrpt::keep_min(inout_tp_obs, new_tp_obs_dist);
424  return;
425  }
426 
427  // Handle the special case of obstacles *inside* the robot at the begining
428  // of the PTG path:
429  switch (COLLISION_BEHAVIOR)
430  {
431  case COLL_BEH_STOP:
432  inout_tp_obs = .0;
433  break;
434 
435  case COLL_BEH_BACK_AWAY:
436  {
437  if (new_tp_obs_dist < getMaxRobotRadius())
438  {
439  // This means that we are getting apart of the obstacle:
440  // ignore it to allow the robot to get off the near-collision:
441  // Don't change inout_tp_obs.
442  return;
443  }
444  else
445  {
446  // This means we are already in collision and trying to get even
447  // closer
448  // to the obstacle: totally disprove this action:
449  inout_tp_obs = .0;
450  }
451  }
452  break;
453 
454  default:
455  THROW_EXCEPTION("Obstacle postprocessing enum not implemented!");
456  }
457 }
458 
460  ClearanceDiagram& cd) const
461 {
463  for (unsigned int decim_k = 0; decim_k < m_clearance_decimated_paths;
464  decim_k++)
465  {
466  const auto real_k = cd.decimated_k_to_real_k(decim_k);
467  const size_t numPathSteps = getPathStepCount(real_k);
468  const double numStepsPerIncr =
469  (numPathSteps - 1.0) / double(m_clearance_num_points);
470 
471  auto& cl_path = cd.get_path_clearance_decimated(decim_k);
472  for (double step_pointer_dbl = 0.0; step_pointer_dbl < numPathSteps;
473  step_pointer_dbl += numStepsPerIncr)
474  {
475  const size_t step = mrpt::round(step_pointer_dbl);
476  const double dist_over_path = this->getPathDist(real_k, step);
477  cl_path[dist_over_path] = 1.0; // create entry in map<>
478  }
479  }
480 }
481 
483  const double ox, const double oy, ClearanceDiagram& cd) const
484 {
485  // Initialize CD on first call:
488 
489  // evaluate in derived-class: this function also keeps the minimum
490  // automatically.
491  for (uint16_t decim_k = 0; decim_k < cd.get_decimated_num_paths();
492  decim_k++)
493  {
494  const auto real_k = cd.decimated_k_to_real_k(decim_k);
496  ox, oy, real_k, cd.get_path_clearance_decimated(decim_k));
497  }
498 }
499 
501  ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const
502 {
503  // Used only when in approx mode (Removed 30/01/2017)
504 }
505 
507  const double ox, const double oy, const uint16_t k,
508  ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
509  bool treat_as_obstacle) const
510 {
511  bool had_collision = false;
512 
513  const size_t numPathSteps = getPathStepCount(k);
514  ASSERT_(numPathSteps > inout_realdist2clearance.size());
515 
516  const double numStepsPerIncr =
517  (numPathSteps - 1.0) / (inout_realdist2clearance.size());
518 
519  double step_pointer_dbl = 0.0;
520  const mrpt::math::TPoint2D og(ox, oy); // obstacle in "global" frame
521  mrpt::math::TPoint2D ol; // obstacle in robot frame
522 
523  for (auto& e : inout_realdist2clearance)
524  {
525  step_pointer_dbl += numStepsPerIncr;
526  const size_t step = mrpt::round(step_pointer_dbl);
527  // const double dist_over_path = e.first;
528  double& inout_clearance = e.second;
529 
530  if (had_collision)
531  {
532  // We found a collision in a previous step along this "k" path, so
533  // it does not make sense to evaluate the clearance of a pose which
534  // is not reachable:
535  inout_clearance = .0;
536  continue;
537  }
538 
539  mrpt::math::TPose2D pose;
540  this->getPathPose(k, step, pose);
541 
542  // obstacle to robot clearance:
543  ol = pose.inverseComposePoint(og);
544  const double this_clearance =
545  treat_as_obstacle ? this->evalClearanceToRobotShape(ol.x, ol.y)
546  : ol.norm();
547  if (this_clearance <= .0 && treat_as_obstacle)
548  {
549  // Collision:
550  had_collision = true;
551  inout_clearance = .0;
552  }
553  else
554  {
555  // The obstacle is not a direct collision.
556  const double this_clearance_norm =
557  this_clearance / this->refDistance;
558 
559  // Update minimum in output structure
560  mrpt::keep_min(inout_clearance, this_clearance_norm);
561  }
562  }
563 }
564 
566  : curVelLocal(0, 0, 0),
567  relTarget(20.0, 0, 0), // Default: assume a "distant" target ahead
568  targetRelSpeed(0)
569 {
570 }
571 
573  const TNavDynamicState& o) const
574 {
575  return (curVelLocal == o.curVelLocal) && (relTarget == o.relTarget) &&
576  (targetRelSpeed == o.targetRelSpeed);
577 }
578 
581 {
582  const uint8_t version = 0;
583  out << version;
584  // Data:
585  out << curVelLocal << relTarget << targetRelSpeed;
586 }
587 
590 {
591  uint8_t version;
592  in >> version;
593  switch (version)
594  {
595  case 0:
596  in >> curVelLocal >> relTarget >> targetRelSpeed;
597  break;
598  default:
600  };
601 }
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.
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
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:3525
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
size_t get_decimated_num_paths() const
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:158
#define MRPT_START
Definition: exceptions.h:262
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:44
IMPLEMENTS_VIRTUAL_SERIALIZABLE(CParameterizedTrajectoryGenerator, CSerializable, mrpt::nav) CParameterizedTrajectoryGenerator
double RAD2DEG(const double x)
Radians to degrees.
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. ...
double x
X,Y coordinates.
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:90
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
double x
X,Y coordinates.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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)...
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
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:3827
GLenum GLsizei n
Definition: glext.h:5074
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
Favor getting back from too-close (almost collision) obstacles.
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
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.
std::string 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:328
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.
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...
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.
bool debugDumpInFiles(const std::string &ptg_name) const
Dump PTG trajectories in four text files: `.
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:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
static std::string OUTPUT_DEBUG_PATH_PREFIX
The path used as defaul output in, for example, debugDumpInFiles.
size_t decimated_k_to_real_k(size_t k) const
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
Definition: CSetOfLines.h:70
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...
size_t get_actual_num_paths() const
double vx
Velocity components: X,Y (m/s)
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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.
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
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...
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
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.
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...
unsigned __int64 uint64_t
Definition: rptypes.h:50
void deinitialize()
This must be called to de-initialize the PTG if some parameter is to be changed.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
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 ...
#define MRPT_END
Definition: exceptions.h:266
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
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...
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:33
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
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:6305
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)
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.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019