MRPT  2.0.3
CParameterizedTrajectoryGenerator.cpp
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 
10 #include "nav-precomp.h" // Precomp header
11 
15 #include <mrpt/system/filesystem.h>
16 #include <mrpt/system/os.h>
17 #include <fstream>
18 
19 using namespace mrpt::nav;
20 
21 static std::string OUTPUT_DEBUG_PATH_PREFIX = "./reactivenav.logs";
24 
26 {
28 }
29 
32 {
34 }
35 
38 
40  : m_nav_dyn_state(), m_nav_dyn_state_target_k(INVALID_PTG_PATH_INDEX)
41 
42 {
43 }
44 
45 void CParameterizedTrajectoryGenerator::loadDefaultParams()
46 {
47  m_alphaValuesCount = 121;
48  refDistance = 6.0;
49  m_score_priority = 1.0;
52 }
53 
55 {
56  return false;
57 }
59 {
60  return .0;
61 }
62 
64  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
65 {
67  num_paths, uint64_t, m_alphaValuesCount, cfg, sSection);
68  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(refDistance, double, cfg, sSection);
70  score_priority, double, m_score_priority, cfg, sSection);
72  clearance_num_points, double, m_clearance_num_points, cfg, sSection);
74  clearance_decimated_paths, double, m_clearance_decimated_paths, cfg,
75  sSection);
76 
77  // Ensure a minimum of resolution:
80  static_cast<decltype(m_clearance_num_points)>(refDistance / 1.0));
81 
82  // Optional params, for debugging only
84  vxi, double, m_nav_dyn_state.curVelLocal.vx, cfg, sSection);
86  vyi, double, m_nav_dyn_state.curVelLocal.vy, cfg, sSection);
88  wi, double, m_nav_dyn_state.curVelLocal.omega, cfg, sSection);
89 
91  reltrg_x, double, m_nav_dyn_state.relTarget.x, cfg, sSection);
93  reltrg_y, double, m_nav_dyn_state.relTarget.y, cfg, sSection);
95  reltrg_phi, double, m_nav_dyn_state.relTarget.phi, cfg, sSection);
96 
98  target_rel_speed, double, m_nav_dyn_state.targetRelSpeed, cfg,
99  sSection);
100 }
102  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
103 {
104  MRPT_START
105  const int WN = 25, WV = 30;
106 
107  cfg.write(
108  sSection, "num_paths", m_alphaValuesCount, WN, WV,
109  "Number of discrete paths (`resolution`) in the PTG");
110  cfg.write(
111  sSection, "refDistance", refDistance, WN, WV,
112  "Maximum distance (meters) for building trajectories (visibility "
113  "range)");
114  cfg.write(
115  sSection, "score_priority", m_score_priority, WN, WV,
116  "When used in path planning, a multiplying factor (default=1.0) for "
117  "the scores for this PTG. Assign values <1 to PTGs with low priority.");
118  cfg.write(
119  sSection, "clearance_num_points", m_clearance_num_points, WN, WV,
120  "Number of steps for the piecewise-constant approximation of clearance "
121  "(Default=5).");
122  cfg.write(
123  sSection, "clearance_decimated_paths", m_clearance_decimated_paths, WN,
124  WV,
125  "Number of decimated paths for estimation of clearance (Default=15).");
126 
127  // Optional params, for debugging only
128  cfg.write(
129  sSection, "vxi", m_nav_dyn_state.curVelLocal.vx, WN, WV,
130  "(Only for debugging) Current robot velocity vx [m/s].");
131  cfg.write(
132  sSection, "vyi", m_nav_dyn_state.curVelLocal.vy, WN, WV,
133  "(Only for debugging) Current robot velocity vy [m/s].");
134  cfg.write(
135  sSection, "wi", mrpt::RAD2DEG(m_nav_dyn_state.curVelLocal.omega), WN,
136  WV, "(Only for debugging) Current robot velocity omega [deg/s].");
137 
138  cfg.write(
139  sSection, "reltrg_x", m_nav_dyn_state.relTarget.x, WN, WV,
140  "(Only for debugging) Relative target x [m].");
141  cfg.write(
142  sSection, "reltrg_y", m_nav_dyn_state.relTarget.y, WN, WV,
143  "(Only for debugging) Relative target y [m].");
144  cfg.write(
145  sSection, "reltrg_phi", mrpt::RAD2DEG(m_nav_dyn_state.relTarget.phi),
146  WN, WV, "(Only for debugging) Relative target phi [deg].");
147 
148  cfg.write(
149  sSection, "target_rel_speed", m_nav_dyn_state.targetRelSpeed, WN, WV,
150  "(Only for debugging) Desired relative speed at target [0,1]");
151 
152  MRPT_END
153 }
154 
157 {
158  this->deinitialize();
159 
160  uint8_t version;
161  in >> version;
162  switch (version)
163  {
164  case 0:
165  case 1:
166  case 2:
167  case 3:
168  case 4:
170  if (version >= 1) in >> m_clearance_num_points;
171  if (version == 2)
172  {
173  bool old_use_approx_clearance;
174  in >> old_use_approx_clearance; // ignored in v>=3
175  }
176  if (version >= 4)
177  {
179  }
180  else
181  {
183  }
184  break;
185  default:
187  };
188 }
189 
192 {
193  const uint8_t version = 4;
194  out << version;
195 
197  << m_clearance_num_points /* v1 */;
198  out << m_clearance_decimated_paths /* v4*/;
199 }
200 
202  uint16_t k, const unsigned int num_paths)
203 {
204  ASSERT_BELOW_(k, num_paths);
205  return M_PI * (-1.0 + 2.0 * (k + 0.5) / num_paths);
206 }
207 
209 {
210  return index2alpha(k, m_alphaValuesCount);
211 }
212 
214  double alpha, const unsigned int num_paths)
215 {
216  mrpt::math::wrapToPi(alpha);
217  int k = mrpt::round(0.5 * (num_paths * (1.0 + alpha / M_PI) - 1.0));
218  if (k < 0) k = 0;
219  if (k >= static_cast<int>(num_paths)) k = num_paths - 1;
220  return (uint16_t)k;
221 }
222 
224 {
225  return alpha2index(alpha, m_alphaValuesCount);
226 }
227 
229  const uint16_t k, mrpt::opengl::CSetOfLines& gl_obj,
230  const double decimate_distance, const double max_path_distance) const
231 {
232  const size_t nPointsInPath = getPathStepCount(k);
233 
234  bool first = true;
235  // Decimate trajectories: we don't need centimeter resolution!
236  double last_added_dist = 0.0;
237  for (size_t n = 0; n < nPointsInPath; n++)
238  {
239  const double d = this->getPathDist(
240  k, n); // distance thru path "k" until timestep "n"
241 
242  // Draw the TP only until we reach the target of the "motion" segment:
243  if (max_path_distance >= 0.0 && d >= max_path_distance) break;
244 
245  if (d < last_added_dist + decimate_distance && n != 0)
246  continue; // skip: decimation
247 
248  last_added_dist = d;
249 
251  this->getPathPose(k, n, p);
252 
253  if (first)
254  {
255  first = false;
256  gl_obj.appendLine(0, 0, 0, p.x, p.y, 0);
257  }
258  else
259  gl_obj.appendLineStrip(p.x, p.y, 0);
260  }
261 }
262 
264  std::vector<double>& TP_Obstacles) const
265 {
266  TP_Obstacles.resize(m_alphaValuesCount);
267  for (size_t k = 0; k < m_alphaValuesCount; k++)
268  initTPObstacleSingle(k, TP_Obstacles[k]);
269 }
271  uint16_t k, double& TP_Obstacle_k) const
272 {
273  TP_Obstacle_k = std::min(
275  ? refDistance
276  : this->getPathDist(k, this->getPathStepCount(k) - 1));
277 }
278 
280  const std::string& ptg_name) const
281 {
282  using namespace mrpt::system;
283  using namespace std;
284 
285  const char* sPath =
287 
289  mrpt::system::createDirectory(mrpt::format("%s/PTGs", sPath));
290 
291  const string sFilTxt_x =
292  mrpt::format("%s/PTGs/PTG%s_x.txt", sPath, ptg_name.c_str());
293  const string sFilTxt_y =
294  mrpt::format("%s/PTGs/PTG%s_y.txt", sPath, ptg_name.c_str());
295  const string sFilTxt_phi =
296  mrpt::format("%s/PTGs/PTG%s_phi.txt", sPath, ptg_name.c_str());
297  const string sFilTxt_t =
298  mrpt::format("%s/PTGs/PTG%s_t.txt", sPath, ptg_name.c_str());
299  const string sFilTxt_d =
300  mrpt::format("%s/PTGs/PTG%s_d.txt", sPath, ptg_name.c_str());
301 
302  ofstream fx(sFilTxt_x.c_str());
303  if (!fx.is_open()) return false;
304  ofstream fy(sFilTxt_y.c_str());
305  if (!fy.is_open()) return false;
306  ofstream fp(sFilTxt_phi.c_str());
307  if (!fp.is_open()) return false;
308  ofstream fd(sFilTxt_d.c_str());
309  if (!fd.is_open()) return false;
310 
311  const size_t nPaths = getAlphaValuesCount();
312 
313  // Text version:
314  fx << "% PTG data file for 'x'. Each row is the trajectory for a different "
315  "'alpha' parameter value."
316  << endl;
317  fy << "% PTG data file for 'y'. Each row is the trajectory for a different "
318  "'alpha' parameter value."
319  << endl;
320  fp << "% PTG data file for 'phi'. Each row is the trajectory for a "
321  "different 'alpha' parameter value."
322  << endl;
323  fd << "% PTG data file for 'd'. Each row is the trajectory for a different "
324  "'alpha' parameter value."
325  << endl;
326 
327  vector<size_t> path_length(nPaths);
328  for (size_t k = 0; k < nPaths; k++) path_length[k] = getPathStepCount(k);
329 
330  size_t maxPoints = 0;
331  for (size_t k = 0; k < nPaths; k++)
332  maxPoints = max(maxPoints, path_length[k]);
333 
334  for (size_t k = 0; k < nPaths; k++)
335  {
336  for (size_t n = 0; n < maxPoints; n++)
337  {
338  const size_t nn = std::min(n, path_length[k] - 1);
340  this->getPathPose(k, nn, p);
341  fx << p.x << " ";
342  fy << p.y << " ";
343  fp << p.phi << " ";
344  fd << this->getPathDist(k, nn) << " ";
345  }
346  fx << endl;
347  fy << endl;
348  fp << endl;
349  fd << endl;
350  }
351 
352  return true;
353 }
354 
356 {
357  return m_is_initialized;
358 }
359 
362  const bool force_update)
363 {
364  // Make sure there is a real difference: notifying a PTG that a
365  // condition changed may imply a significant computational cost if paths
366  // need to be re-evaluated on the fly, etc. so the cost of the
367  // comparison here is totally worth:
368  if (force_update || m_nav_dyn_state != newState)
369  {
370  ASSERT_(
371  newState.targetRelSpeed >= .0 &&
372  newState.targetRelSpeed <= 1.0); // sanity check
373  m_nav_dyn_state = newState;
374 
375  // 1st) Build PTG paths without counting for target slow-down:
377 
378  this->onNewNavDynamicState();
379 
380  // 2nd) Save the special path for slow-down:
381  if (this->supportSpeedAtTarget())
382  {
383  int target_k = -1;
384  double target_norm_d;
385  // bool is_exact = // JLB removed this constraint for being too
386  // restrictive.
387  this->inverseMap_WS2TP(
389  target_k, target_norm_d, 1.0 /*large tolerance*/);
390  if (target_norm_d > 0.01 && target_norm_d < 0.99 && target_k >= 0 &&
391  target_k < m_alphaValuesCount)
392  {
393  m_nav_dyn_state_target_k = target_k;
394  this->onNewNavDynamicState(); // Recalc
395  }
396  }
397  }
398 }
399 
401  const std::string& cacheFilename, const bool verbose)
402 {
403  if (m_is_initialized) return;
404 
405  const std::string sCache =
406  !cacheFilename.empty()
407  ? cacheFilename
408  : std::string("cache_") +
410  std::string(".bin.gz");
411 
412  this->internal_initialize(sCache, verbose);
413  m_is_initialized = true;
414 }
416 {
417  if (!m_is_initialized) return;
418  this->internal_deinitialize();
419  m_is_initialized = false;
420 }
421 
423  const double ox, const double oy, const double new_tp_obs_dist,
424  double& inout_tp_obs) const
425 {
426  const bool is_obs_inside_robot_shape = isPointInsideRobotShape(ox, oy);
427  if (!is_obs_inside_robot_shape)
428  {
429  mrpt::keep_min(inout_tp_obs, new_tp_obs_dist);
430  return;
431  }
432 
433  // Handle the special case of obstacles *inside* the robot at the begining
434  // of the PTG path:
435  switch (COLLISION_BEHAVIOR())
436  {
437  case COLL_BEH_STOP:
438  inout_tp_obs = .0;
439  break;
440 
441  case COLL_BEH_BACK_AWAY:
442  {
443  if (new_tp_obs_dist < getMaxRobotRadius())
444  {
445  // This means that we are getting apart of the obstacle:
446  // ignore it to allow the robot to get off the near-collision:
447  // Don't change inout_tp_obs.
448  return;
449  }
450  else
451  {
452  // This means we are already in collision and trying to get even
453  // closer
454  // to the obstacle: totally disprove this action:
455  inout_tp_obs = .0;
456  }
457  }
458  break;
459 
460  default:
461  THROW_EXCEPTION("Obstacle postprocessing enum not implemented!");
462  }
463 }
464 
466  ClearanceDiagram& cd) const
467 {
469  for (unsigned int decim_k = 0; decim_k < m_clearance_decimated_paths;
470  decim_k++)
471  {
472  const auto real_k = cd.decimated_k_to_real_k(decim_k);
473  const size_t numPathSteps = getPathStepCount(real_k);
474  const double numStepsPerIncr =
475  (numPathSteps - 1.0) / double(m_clearance_num_points);
476 
477  auto& cl_path = cd.get_path_clearance_decimated(decim_k);
478  for (double step_pointer_dbl = 0.0; step_pointer_dbl < numPathSteps;
479  step_pointer_dbl += numStepsPerIncr)
480  {
481  const size_t step = mrpt::round(step_pointer_dbl);
482  const double dist_over_path = this->getPathDist(real_k, step);
483  cl_path[dist_over_path] = 1.0; // create entry in map<>
484  }
485  }
486 }
487 
489  const double ox, const double oy, ClearanceDiagram& cd) const
490 {
491  // Initialize CD on first call:
494 
495  // evaluate in derived-class: this function also keeps the minimum
496  // automatically.
497  for (uint16_t decim_k = 0; decim_k < cd.get_decimated_num_paths();
498  decim_k++)
499  {
500  const auto real_k = cd.decimated_k_to_real_k(decim_k);
502  ox, oy, real_k, cd.get_path_clearance_decimated(decim_k));
503  }
504 }
505 
507  ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const
508 {
509  // Used only when in approx mode (Removed 30/01/2017)
510 }
511 
513  const double ox, const double oy, const uint16_t k,
514  ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
515  bool treat_as_obstacle) const
516 {
517  bool had_collision = false;
518 
519  const size_t numPathSteps = getPathStepCount(k);
520  // We don't have steps enough (?). Just ignore clearance for this short
521  // path in this "k" direction:
522  if (numPathSteps <= inout_realdist2clearance.size())
523  {
524  std::cerr << "[CParameterizedTrajectoryGenerator::"
525  "evalClearanceSingleObstacle] Warning: k="
526  << k << " numPathSteps is only=" << numPathSteps
527  << " num of clearance steps="
528  << inout_realdist2clearance.size();
529  return;
530  }
531 
532  const double numStepsPerIncr =
533  (numPathSteps - 1.0) / (inout_realdist2clearance.size());
534 
535  double step_pointer_dbl = 0.0;
536  const mrpt::math::TPoint2D og(ox, oy); // obstacle in "global" frame
537  mrpt::math::TPoint2D ol; // obstacle in robot frame
538 
539  for (auto& e : inout_realdist2clearance)
540  {
541  step_pointer_dbl += numStepsPerIncr;
542  const size_t step = mrpt::round(step_pointer_dbl);
543  const double dist_over_path = e.first;
544  double& inout_clearance = e.second;
545 
546  if (had_collision)
547  {
548  // We found a collision in a previous step along this "k" path, so
549  // it does not make sense to evaluate the clearance of a pose which
550  // is not reachable:
551  inout_clearance = .0;
552  continue;
553  }
554 
555  mrpt::math::TPose2D pose;
556  this->getPathPose(k, step, pose);
557 
558  // obstacle to robot clearance:
559  ol = pose.inverseComposePoint(og);
560  const double this_clearance =
561  treat_as_obstacle ? this->evalClearanceToRobotShape(ol.x, ol.y)
562  : ol.norm();
563  if (this_clearance <= .0 && treat_as_obstacle &&
564  (dist_over_path > 0.5 ||
565  std::abs(mrpt::math::angDistance(
566  std::atan2(oy, ox), index2alpha(k))) < mrpt::DEG2RAD(45.0)))
567  {
568  // Collision:
569  had_collision = true;
570  inout_clearance = .0;
571  }
572  else
573  {
574  // The obstacle is not a direct collision.
575  const double this_clearance_norm =
576  this_clearance / this->refDistance;
577 
578  // Update minimum in output structure
579  mrpt::keep_min(inout_clearance, this_clearance_norm);
580  }
581  }
582 }
583 
585  uint16_t k, uint32_t step) const
586 {
587  if (step > 0)
588  {
589  // Numerical estimate of "global" (wrt current, start of path) direction
590  // of motion:
591  const auto curPose = getPathPose(k, step);
592  const auto prevPose = getPathPose(k, step - 1);
593  const double dt = getPathStepDuration();
594  ASSERT_ABOVE_(dt, .0);
595 
596  auto vel = mrpt::math::TTwist2D(
597  curPose.x - prevPose.x, curPose.y - prevPose.y,
598  mrpt::math::angDistance(prevPose.phi, curPose.phi));
599  vel *= (1.0 / dt);
600  return vel;
601  }
602  else
603  {
604  // Initial velocity:
606  }
607 }
608 
610  const TNavDynamicState& o) const
611 {
612  return (curVelLocal == o.curVelLocal) && (relTarget == o.relTarget) &&
613  (targetRelSpeed == o.targetRelSpeed);
614 }
615 
618 {
619  const uint8_t version = 0;
620  out << version;
621  // Data:
622  out << curVelLocal << relTarget << targetRelSpeed;
623 }
624 
627 {
628  uint8_t version;
629  in >> version;
630  switch (version)
631  {
632  case 0:
633  in >> curVelLocal >> relTarget >> targetRelSpeed;
634  break;
635  default:
637  };
638 }
CSetOfLines.h
os.h
filesystem.h
nav-precomp.h
CParameterizedTrajectoryGenerator.h
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle
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 ...
Definition: CParameterizedTrajectoryGenerator.cpp:512
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::readFromStream
void readFromStream(mrpt::serialization::CArchive &in)
Definition: CParameterizedTrajectoryGenerator.cpp:626
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::operator==
bool operator==(const TNavDynamicState &o) const
Definition: CParameterizedTrajectoryGenerator.cpp:609
mrpt::nav::CParameterizedTrajectoryGenerator::internal_TPObsDistancePostprocess
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...
Definition: CParameterizedTrajectoryGenerator.cpp:422
mrpt::nav::ClearanceDiagram::get_actual_num_paths
size_t get_actual_num_paths() const
Definition: ClearanceDiagram.h:40
mrpt::nav::CParameterizedTrajectoryGenerator::OUTPUT_DEBUG_PATH_PREFIX
static std::string & OUTPUT_DEBUG_PATH_PREFIX()
The path used as defaul output in, for example, debugDumpInFiles.
Definition: CParameterizedTrajectoryGenerator.cpp:25
mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_num_points
uint16_t m_clearance_num_points
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0,...
Definition: CParameterizedTrajectoryGenerator.h:470
mrpt::config::CConfigFileBase::write
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())
Definition: config/CConfigFileBase.h:107
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::targetRelSpeed
double targetRelSpeed
Desired relative speed [0,1] at target.
Definition: CParameterizedTrajectoryGenerator.h:176
verbose
params verbose
Definition: chessboard_stereo_camera_calib_unittest.cpp:59
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: TPose2D.h:32
OUTPUT_DEBUG_PATH_PREFIX
static std::string OUTPUT_DEBUG_PATH_PREFIX
Definition: CParameterizedTrajectoryGenerator.cpp:21
mrpt::math::TPoint2D_< double >
mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
Definition: CParameterizedTrajectoryGenerator.cpp:155
mrpt::opengl::CSetOfLines
A set of independent lines (or segments), one line with its own start and end positions (X,...
Definition: CSetOfLines.h:32
mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceToRobotShape
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.
mrpt::math::angDistance
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
Definition: wrap2pi.h:95
mrpt::nav::ClearanceDiagram::get_decimated_num_paths
size_t get_decimated_num_paths() const
Definition: ClearanceDiagram.h:41
mrpt::nav::CParameterizedTrajectoryGenerator::INVALID_PTG_PATH_INDEX
static const uint16_t INVALID_PTG_PATH_INDEX
Definition: CParameterizedTrajectoryGenerator.h:479
mrpt::math::TPose2D::inverseComposePoint
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
Definition: TPose2D.cpp:74
mrpt::nav::CParameterizedTrajectoryGenerator::updateClearance
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...
Definition: CParameterizedTrajectoryGenerator.cpp:488
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::nav::CParameterizedTrajectoryGenerator::deinitialize
void deinitialize()
This must be called to de-initialize the PTG if some parameter is to be changed.
Definition: CParameterizedTrajectoryGenerator.cpp:415
mrpt::math::TPose2D::y
double y
Definition: TPose2D.h:30
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_decimated_paths
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
Definition: CParameterizedTrajectoryGenerator.h:472
mrpt::math::TTwist2D::vx
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
mrpt::nav::CParameterizedTrajectoryGenerator::internal_deinitialize
virtual void internal_deinitialize()=0
This must be called to de-initialize the PTG if some parameter is to be changed.
mrpt::math::TTwist2D::vy
double vy
Definition: TTwist2D.h:26
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
MRPT_LOAD_HERE_CONFIG_VAR_DEGREES
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:376
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::relTarget
mrpt::math::TPose2D relTarget
Current relative target location.
Definition: CParameterizedTrajectoryGenerator.h:174
mrpt::nav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP
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...
mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
Definition: CParameterizedTrajectoryGenerator.cpp:208
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:358
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:401
mrpt::system::fileNameStripInvalidChars
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char.
Definition: filesystem.cpp:329
mrpt::math::TPoint2D_::norm
T norm() const
Point norm: |v| = sqrt(x^2+y^2)
Definition: TPoint2D.h:205
mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
Definition: CParameterizedTrajectoryGenerator.cpp:223
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::curVelLocal
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
Definition: CParameterizedTrajectoryGenerator.h:172
mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state
TNavDynamicState m_nav_dyn_state
Updated before each nav step by
Definition: CParameterizedTrajectoryGenerator.h:474
ASSERT_BELOW_
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
mrpt::opengl::CSetOfLines::appendLine
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
Definition: CSetOfLines.h:71
mrpt::opengl::CSetOfLines::appendLineStrip
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's GL_LINE_ST...
Definition: CSetOfLines.h:91
MRPT_LOAD_HERE_CONFIG_VAR
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:352
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
mrpt::nav::CParameterizedTrajectoryGenerator::supportSpeedAtTarget
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
Definition: CParameterizedTrajectoryGenerator.h:291
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::nav::CParameterizedTrajectoryGenerator::initialize
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...
Definition: CParameterizedTrajectoryGenerator.cpp:400
mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
Definition: CParameterizedTrajectoryGenerator.cpp:263
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
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::CParameterizedTrajectoryGenerator::internal_initialize
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...
mrpt::nav::ClearanceDiagram
Clearance information for one particular PTG and one set of obstacles.
Definition: ClearanceDiagram.h:28
mrpt::nav::CParameterizedTrajectoryGenerator::getMaxRobotRadius
virtual double getMaxRobotRadius() const =0
Returns an approximation of the robot radius.
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
Definition: CParameterizedTrajectoryGenerator.h:466
mrpt::nav::ClearanceDiagram::dist2clearance_t
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
Definition: ClearanceDiagram.h:62
mrpt::nav::CParameterizedTrajectoryGenerator::getDescription
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState
Dynamic state that may affect the PTG path parameterization.
Definition: CParameterizedTrajectoryGenerator.h:169
IMPLEMENTS_VIRTUAL_SERIALIZABLE
IMPLEMENTS_VIRTUAL_SERIALIZABLE(CParameterizedTrajectoryGenerator, CSerializable, mrpt::nav) CParameterizedTrajectoryGenerator
Definition: CParameterizedTrajectoryGenerator.cpp:36
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::nav::CParameterizedTrajectoryGenerator::refDistance
double refDistance
Definition: CParameterizedTrajectoryGenerator.h:464
mrpt::nav::ClearanceDiagram::decimated_k_to_real_k
size_t decimated_k_to_real_k(size_t k) const
Definition: ClearanceDiagram.cpp:123
mrpt::keep_max
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.
Definition: core/include/mrpt/core/bits_math.h:152
mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
Definition: CParameterizedTrajectoryGenerator.cpp:190
mrpt::keep_min
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.
Definition: core/include/mrpt/core/bits_math.h:145
mrpt::math::TPoint2D_data::y
T y
Definition: TPoint2D.h:25
mrpt::nav::CParameterizedTrajectoryGenerator::supportVelCmdNOP
virtual bool supportVelCmdNOP() const
Returns true if it is possible to stop sending velocity commands to the robot and,...
Definition: CParameterizedTrajectoryGenerator.cpp:54
mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
Definition: CParameterizedTrajectoryGenerator.cpp:270
mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepDuration
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::writeToStream
void writeToStream(mrpt::serialization::CArchive &out) const
Definition: CParameterizedTrajectoryGenerator.cpp:617
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::nav::COLL_BEH_BACK_AWAY
@ COLL_BEH_BACK_AWAY
Favor getting back from too-close (almost collision) obstacles.
Definition: CParameterizedTrajectoryGenerator.h:44
mrpt::nav::CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR
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...
Definition: CParameterizedTrajectoryGenerator.cpp:31
mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose
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.
mrpt::nav::CParameterizedTrajectoryGenerator::getPathTwist
virtual mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const
Gets velocity ("twist") for path k ([0,N-1]=>[-pi,pi] in alpha), at vehicle discrete step step.
Definition: CParameterizedTrajectoryGenerator.cpp:584
mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepCount
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.
mrpt::math::TPoint2D_data::x
T x
X,Y coordinates.
Definition: TPoint2D.h:25
mrpt::nav::CParameterizedTrajectoryGenerator::renderPathAsSimpleLine
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)...
Definition: CParameterizedTrajectoryGenerator.cpp:228
mrpt::nav::PTG_collision_behavior_t
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
Definition: CParameterizedTrajectoryGenerator.h:41
mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist
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.
COLLISION_BEHAVIOR
static PTG_collision_behavior_t COLLISION_BEHAVIOR
Definition: CParameterizedTrajectoryGenerator.cpp:22
mrpt::nav::CParameterizedTrajectoryGenerator::maxTimeInVelCmdNOP
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
Definition: CParameterizedTrajectoryGenerator.cpp:58
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::nav::CParameterizedTrajectoryGenerator::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: CParameterizedTrajectoryGenerator.cpp:101
mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state_target_k
uint16_t m_nav_dyn_state_target_k
Update in updateNavDynamicState(), contains the path index (k) for the target.
Definition: CParameterizedTrajectoryGenerator.h:477
mrpt::nav::CParameterizedTrajectoryGenerator::updateClearancePost
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
Definition: CParameterizedTrajectoryGenerator.cpp:506
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::nav::CParameterizedTrajectoryGenerator::isPointInsideRobotShape
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState
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...
Definition: CParameterizedTrajectoryGenerator.cpp:360
mrpt::nav::COLL_BEH_STOP
@ COLL_BEH_STOP
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
Definition: CParameterizedTrajectoryGenerator.h:47
mrpt::nav::CParameterizedTrajectoryGenerator::isInitialized
bool isInitialized() const
Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queri...
Definition: CParameterizedTrajectoryGenerator.cpp:355
mrpt::nav::ClearanceDiagram::resize
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...
Definition: ClearanceDiagram.cpp:180
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::system::createDirectory
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
mrpt::nav::CParameterizedTrajectoryGenerator::m_score_priority
double m_score_priority
Definition: CParameterizedTrajectoryGenerator.h:467
mrpt::nav::CParameterizedTrajectoryGenerator::debugDumpInFiles
bool debugDumpInFiles(const std::string &ptg_name) const
Dump PTG trajectories in four text files: .
Definition: CParameterizedTrajectoryGenerator.cpp:279
mrpt::math::TTwist2D::omega
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: TPose2D.h:30
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:78
mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
Definition: CParameterizedTrajectoryGenerator.cpp:63
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::nav::CParameterizedTrajectoryGenerator::m_is_initialized
bool m_is_initialized
Definition: CParameterizedTrajectoryGenerator.h:481
mrpt::system
Definition: backtrace.h:14
mrpt::nav::CParameterizedTrajectoryGenerator::getAlphaValuesCount
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
Definition: CParameterizedTrajectoryGenerator.h:358
mrpt::nav::CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator
CParameterizedTrajectoryGenerator()
Default ctor.
mrpt::nav::ClearanceDiagram::get_path_clearance_decimated
dist2clearance_t & get_path_clearance_decimated(size_t decim_k)
Definition: ClearanceDiagram.h:66
mrpt::nav::CParameterizedTrajectoryGenerator::onNewNavDynamicState
virtual void onNewNavDynamicState()=0
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
mrpt::math::TTwist2D
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance()
Definition: CParameterizedTrajectoryGenerator.cpp:465



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020