MRPT  1.9.9
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 }
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).
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...
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
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...
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
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...
IMPLEMENTS_VIRTUAL_SERIALIZABLE(CParameterizedTrajectoryGenerator, CSerializable, mrpt::nav) CParameterizedTrajectoryGenerator
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. ...
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
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:91
double x
X,Y coordinates.
Definition: TPose2D.h:30
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
dist2clearance_t & get_path_clearance_decimated(size_t decim_k)
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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:149
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
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.
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:95
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:329
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...
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.
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
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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...
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:71
size_t get_actual_num_paths() const
constexpr double DEG2RAD(const double x)
Degrees to radians.
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
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...
static std::string OUTPUT_DEBUG_PATH_PREFIX
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
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...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
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...
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 getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
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...
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:54
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
mrpt::vision::TStereoCalibResults out
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 ...
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
Lightweight 2D pose.
Definition: TPose2D.h:22
static std::string & OUTPUT_DEBUG_PATH_PREFIX()
The path used as defaul output in, for example, debugDumpInFiles.
T norm() const
Point norm: |v| = sqrt(x^2+y^2)
Definition: TPoint2D.h:205
#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...
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
Definition: TPose2D.cpp:74
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:32
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
TNavDynamicState m_nav_dyn_state
Updated before each nav step by.
static PTG_collision_behavior_t COLLISION_BEHAVIOR
Dynamic state that may affect the PTG path parameterization.
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)
Definition: TPose2D.h:32
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)
Definition: TTwist2D.h:28
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:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020